#include "visionmatch.h"

#include <stdio.h>
#include <iostream>
#ifdef D_USE_OPENCV
#include "opencv2/core/core.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/legacy/legacy.hpp"
#endif
#include "frames.hpp"
#include <qdebug.h>
#include "linematch.h"
#include "domparser.h"
#include "messagelog.h"
#include <Eigen/Geometry>
#include "filteringalgorithm.h"
#include "kalmanfiltersimple.h"

using namespace cv;
using namespace std;

//2^31-1
#define D_MAX_INTEGE 2147483647



VisionMatch::VisionMatch(QString configFileIn, int robotIdIn, int laserSkipCountIn)
{
    configFile=configFileIn;
    robotId=robotIdIn;
    laserSkipCount=laserSkipCountIn;

    minConsumed=100000000000;
    currentConsumed=0;
    maxConsumed=0;
    currentVmarkType=1;

    vmarkInWorldPosition.isUpdateSucess=false;
    vmarkInWorldPosition.matchScore=0;
    vmarkInWorldPosition.maxMatchError=-1;
//    vmarkInWorldPosition.matchSucessTimeStmap=0;
    vmarkInWorldPosition.position.x=0;
    vmarkInWorldPosition.position.y=0;
    vmarkInWorldPosition.position.rotate=0;
    laserDateReadyCount=false;

//    baseMap=new Mat;
//    baseMap_originSource=new Mat;
//    baseCompareMap=new Mat;
//    localMap=new Mat;
//    localCompareMap=new Mat;
//    transMat=new Mat;
    isMatchSucess=false;

    isPrintVmarkProcessData=false;

    frame_baselink_in_laserlink=new KDL::Frame;
    frame_laserlink_in_baselink=new KDL::Frame;
    frame_baselink_in_odom=new KDL::Frame;
    frame_odom_in_map=new KDL::Frame;

    filtering_vmarkPosition_x=new FilteringAlgorithm;
    filtering_vmarkPosition_y=new FilteringAlgorithm;
    filtering_vmarkPosition_r=new FilteringAlgorithm;
    kalmanFilter=new KalmanFilterSimple(0.001,0.05);

    localMap_originX=0;
    localMap_originY=0;
    localMap_originR=0;

    baseMap_originX=0;
    baseMap_originY=0;
    baseMap_originR=0;
    detectFailedType=E_DETECT_FAILED_TYPE_NONE;

    mPerPix=0.025;
//    mPerPix=0.005;//?

    calculationParemeter.method=E_MATCH_CALCULATE_METHOD_VMARK;
    calculationParemeter.keyPointFactor=500;
    calculationParemeter.matchFactor=0.8;
    calculationParemeter.minScale=-1.5;
    calculationParemeter.maxScale=1.5;

    matchDebugInfo.currentLaserFilterType=-1;

    lineMatch=new LineMatch;
    lineMatch->setLineDetectPara(1, M_PI/360.0, 6, 20, 5);

//    int tmpReturn = readVMarkConfigFile(configFileIn,vmark_detect_Parameter);
//    if(1!=tmpReturn)
//    {
//        qDebug()<<"error,VisionMatch readConfigFile";
//        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8230, robotId );
//    }
    int tmpReturn = readStructLineConfigFile(configFileIn,structLine_detect_Parameter,laserFilter_Parameter);
    if(1!=tmpReturn)
    {
        qDebug()<<"error,VisionMatch readConfigFile";
        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8230, robotId );
    }

    poseErrorInfo.x=0;
    poseErrorInfo.y=0;
    poseErrorInfo.rotate=0;//0.038rad=2.17degree

    //---------初始化线程锁--------
    pthread_mutexattr_init(&matchAttr);
    pthread_mutexattr_setprotocol(&matchAttr,PTHREAD_PRIO_INHERIT);
    pthread_mutex_init(&matchMutex,&matchAttr);


}

int VisionMatch::getVmarkPositionCertainity(VehiclePosition &uncertainPoseOut)
{
    uncertainPoseOut=poseErrorInfo;
    return 1;
}

int VisionMatch::setWheelOdometerData_noRos(OdemeterData odemeterDataIn)
{
    pthread_mutex_lock(&matchMutex);

    odemeterData=odemeterDataIn;
    frame_baselink_in_odom->p[0]=odemeterDataIn.pos.x;
    frame_baselink_in_odom->p[1]=odemeterDataIn.pos.y;
    frame_baselink_in_odom->p[2]=0;
    frame_baselink_in_odom->M=frame_baselink_in_odom->M.RPY(0,0,odemeterDataIn.pos.rotate);
    pthread_mutex_unlock(&matchMutex);
    return 1;
}

int VisionMatch::setKalmanFilterPara(int filterId,double followQ, double observeR)
{
    qDebug()<<"VisionMatch::setKalmanFilterPara"<<filterId<<followQ<<observeR;
    switch(filterId)
    {
    case 0:
    {
        return kalmanFilter->setKalmanFilterPara(followQ,followQ);
    }
    }
    return 0;
}

int VisionMatch::setLineFittingMethod(int methodIn)
{
    lineMatch->setLineFittingMethod(methodIn);
    return 1;
}

int VisionMatch::setIsPrintVmarkProcessData(bool isPrint)
{
    isPrintVmarkProcessData=isPrint;
    lineMatch->setIsPrintVmarkProcessData(isPrint);
    return 1;
}

int VisionMatch::laserFilter_test(const LaserDataInfo &frontLaserDataIn, LaserDataInfo &frontLaserDataOut)
{
    frontLaserDataOut=frontLaserDataIn;
    for(int i=0;i<frontLaserDataOut.intensities.size();i++)
    {
//        double tmpAngle=frontLaserDataOut.angle_min+i*frontLaserDataOut.angle_increment;
//        if(frontLaserDataOut.intensities[i]<minumIntensityIn || frontLaserDataOut.ranges[i]>maxLaserRangeIn
//                || tmpAngle<minAngleIn || tmpAngle> maxAngleIn)
        double tmpx=frontLaserDataIn.ranges[i]*cos(frontLaserDataIn.angle_min+frontLaserDataIn.angle_increment*i);
        double tmpy=frontLaserDataIn.ranges[i]*sin(frontLaserDataIn.angle_min+frontLaserDataIn.angle_increment*i);
        if(false==isLaserPointOk_region_test(tmpx,tmpy))
        {
            frontLaserDataOut.ranges[i]=frontLaserDataOut.range_max+10000;
        }
    }

    return 1;
}

bool VisionMatch::isLaserPointOk_region_test(double x_in, double y_in)
{
    MathPoint testPoint;
    testPoint.x=x_in;
    testPoint.y=y_in;
    lineMatch->isLaserPointOk_region(testPoint);
}

int VisionMatch::getVMarkPositionInWorld(VehiclePosition &positionOut, bool &isSucess)
{
    positionOut=vmarkInWorldPosition.position;
    isSucess=vmarkInWorldPosition.isUpdateSucess;
    return 1;
}

int VisionMatch::setReferenceBaselinkPosition(VehiclePosition positionIn)
{
    baselinkReferencePosition=positionIn;
    return 1;
}

int VisionMatch::setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn)
{
    return lineMatch->setLaserIntensityFilter(minumIntensityIn,maxLaserRangeIn,minAngleIn,maxAngleIn);
}

int VisionMatch::setLaserIntensityFilterByType(int typeIn)
{
    if(matchDebugInfo.currentLaserFilterType==typeIn)
    {
       return 1;
    }

    if(true==laserFilter_Parameter.contains(typeIn))
    {
        lineMatch->setLaserIntensityFilter(laserFilter_Parameter.value(typeIn).minumIntensity,
                                           laserFilter_Parameter.value(typeIn).maxLaserRange,
                                           laserFilter_Parameter.value(typeIn).minAngle*M_PI/180.0,
                                           laserFilter_Parameter.value(typeIn).maxAngle*M_PI/180.0);
        std::vector<double> region_x;
        std::vector<double> region_y;
        pthread_mutex_lock(&matchMutex);
        translateToRegion(laserFilter_Parameter.value(typeIn).region_front,
                          laserFilter_Parameter.value(typeIn).region_back,
                          laserFilter_Parameter.value(typeIn).region_left,
                          laserFilter_Parameter.value(typeIn).region_right,region_x,region_y);
        pthread_mutex_unlock(&matchMutex);
        lineMatch->setLaserValidRegion(region_x,region_y);
        matchDebugInfo.currentLaserFilterType=typeIn;
    }
    else
    {
        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8237, robotId ,typeIn);
        return -1;
    }
    return 1;
}

int VisionMatch::setLaserSensorData(const LaserDataInfo *laserData)
{
    pthread_mutex_lock(&matchMutex);
    laserData_received=*laserData;
    laserDateReadyCount++;
    pthread_mutex_unlock(&matchMutex);
    return 1;
}

int VisionMatch::detectReflectLine(PositionInfo &vehiclePositionInfoOut)
{
    if(laserDateReadyCount<=laserSkipCount)
    {
        calculateAgvPosition_byOdom(agvInMapPosition);
        vehiclePositionInfoOut=agvInMapPosition;
        return 1;
    }



    std::vector<double> x_out,y_out,r_out;

//   return lineMatch->detectReflectLine(laserData,x_out,y_out,r_out);
    //时间分析
    struct timespec startTime,endTime;
    clock_gettime(CLOCK_MONOTONIC, &startTime);

    pthread_mutex_lock(&matchMutex);
    laserDateReadyCount=false;
    if(true==structLine_detect_Parameter.contains(currentVmarkType))
    {
        lineMatch->detectTwoLine(structLine_detect_Parameter.value(currentVmarkType),laserData_received,x_out,y_out,r_out,
                                 matchDebugInfo.struct_Line1Count,matchDebugInfo.struct_Line2Count,
                                 matchDebugInfo.distanceOfLinesOut,matchDebugInfo.angleOfLinesOut ,
                                  matchDebugInfo.struct_Line1Corelation,matchDebugInfo.struct_Line2Corelation);
        matchDebugInfo.detectedVmarkResultCount=x_out.size();
        if(1==x_out.size())
        {
            //必须在求逆向矩阵前滤波，否则角度误差耦合到了ｘ\y方向。
//            filterPositionResult(x_out[0],y_out[0],r_out[0]);
            VehiclePosition vmarkInLaser;
            vmarkInLaser.x=x_out[0];
            vmarkInLaser.y=y_out[0];
            vmarkInLaser.rotate=r_out[0];
            calculateVmarkPositionInWorld(vmarkInLaser,vmarkInWorldPosition.position);
            vmarkInWorldPosition.isUpdateSucess=true;

            translate_structLine_in_laserlink_2_baselink_in_structLine(x_out[0],y_out[0],r_out[0],true
                    ,x_out[0],y_out[0],r_out[0]);
            matchDebugInfo.isMatchSucess=true;

            agvInMapPosition.position.x =x_out[0];
            agvInMapPosition.position.y =y_out[0];
            agvInMapPosition.position.rotate=r_out[0];
            clock_gettime(CLOCK_MONOTONIC, &agvInMapPosition.matchSucessTimeStmap);
            agvInMapPosition.matchScore=100;
            agvInMapPosition.maxMatchError=0;
            agvInMapPosition.isUpdateSucess=true;
            matchDebugInfo.pos_x=x_out[0];
            matchDebugInfo.pos_y=y_out[0];
            matchDebugInfo.pos_r=r_out[0];
            //求校正矩阵
            calculateOffsetMatrix(agvInMapPosition.position,frame_baselink_in_odom,frame_odom_in_map);
            poseErrorInfo.x=0;
            poseErrorInfo.y=0;
            poseErrorInfo.rotate=0;
        }
        else
        {
            if(1<x_out.size())
            {
                detectFailedType=E_DETECT_FAILED_TYPE_TOO_MANY_GOOD;
            }
            if(0==matchDebugInfo.struct_Line1Count)
            {
                detectFailedType=E_DETECT_FAILED_TYPE_L1_ZERO;
            }
            else if(0==matchDebugInfo.struct_Line2Count)
            {
                detectFailedType=E_DETECT_FAILED_TYPE_L2_ZERO;
            }
            vmarkInWorldPosition.isUpdateSucess=false;
             matchDebugInfo.isMatchSucess=false;

             agvInMapPosition.matchScore=0;
             agvInMapPosition.isUpdateSucess=false;
        }

    }
    else
    {
        vmarkInWorldPosition.isUpdateSucess=false;
        matchDebugInfo.isMatchSucess=false;
        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8236, robotId ,currentVmarkType);
    }
    pthread_mutex_unlock(&matchMutex);

    clock_gettime(CLOCK_MONOTONIC, &endTime);
    currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
    currentConsumed=currentConsumed/1000.0;
    if(currentConsumed<minConsumed)
    {
        minConsumed=currentConsumed;
    }
    if(currentConsumed>maxConsumed)
    {
        maxConsumed=currentConsumed;
    }
    matchDebugInfo.minConsumed=minConsumed;
    matchDebugInfo.currentConsumed=currentConsumed;
    matchDebugInfo.maxConsumed=maxConsumed;
    return 1;
}


int VisionMatch::setLaserLinkInBaseLink(double x, double y, double z, double roll, double pitch, double yaw)
{
    pthread_mutex_lock(&matchMutex);
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(roll,pitch,yaw);
    laser_baslink_pitch=pitch;
    KDL::Vector tmpVector(x,y,z);
    *frame_laserlink_in_baselink =KDL::Frame(tmpRotation,tmpVector);
    *frame_baselink_in_laserlink=frame_laserlink_in_baselink->Inverse();
    pthread_mutex_unlock(&matchMutex);

    setLaserIntensityFilterByType(0);//默认使用０号
    return 1;
}

int VisionMatch::getTimeConsumed(double &minConsumedOut, double &currentConsumedOut, double &maxConsumedOut)
{
    minConsumedOut=minConsumed;
    currentConsumedOut=currentConsumed;
    maxConsumedOut=maxConsumed;
    return 1;

}

int VisionMatch::resetRosMeasureTime()
{
    minConsumed=100000;
    maxConsumed=-100000;
    return 1;
}

int VisionMatch::setCurrentVMarkIndex(int indexIn)
{
    currentVmarkIndex=indexIn;
    return 1;
}

int VisionMatch::setCurrentVMarkType(int currentVmarkTypeIn)
{
    filtering_vmarkPosition_x->resetAll();
    filtering_vmarkPosition_y->resetAll();
    filtering_vmarkPosition_r->resetAll();
//    pthread_mutex_lock(&matchMutex);
//    currentVmarkIndex=indexIn;
    if(currentVmarkType!=currentVmarkTypeIn)
    {
//        filtering_vmarkPosition_x->resetAll();
//        filtering_vmarkPosition_y->resetAll();
//        filtering_vmarkPosition_r->resetAll();
    }
    currentVmarkType=currentVmarkTypeIn;
    matchDebugInfo.currentVmarkType=currentVmarkType;
//    if(true==vmark_detect_Parameter.contains(currentVmarkTypeIn))
//    {
//        lineMatch->setLineDetectPara(vmark_detect_Parameter[currentVmarkType].rho_distanceAccuracy,
//                                     vmark_detect_Parameter[currentVmarkType].theta_angleAccuracy,
//                                     vmark_detect_Parameter[currentVmarkType].threshold_pt,
//                                     vmark_detect_Parameter[currentVmarkType].minLineLength,
//                                     vmark_detect_Parameter[currentVmarkType].maxLineGap);
//    }
//    pthread_mutex_unlock(&matchMutex);
    return 1;
}

int VisionMatch::setVmarkDetectParameter(VMarkDetectParameter vmarkParameterIn)
{
    pthread_mutex_lock(&matchMutex);
    if(true==vmark_detect_Parameter.contains(currentVmarkType))
    {
       vmark_detect_Parameter[currentVmarkType]=vmarkParameterIn;
    }

    lineMatch->setLineDetectPara(vmark_detect_Parameter[currentVmarkType].rho_distanceAccuracy,
                                 vmark_detect_Parameter[currentVmarkType].theta_angleAccuracy,
                                 vmark_detect_Parameter[currentVmarkType].threshold_pt,
                                 vmark_detect_Parameter[currentVmarkType].minLineLength,
                                 vmark_detect_Parameter[currentVmarkType].maxLineGap);
    pthread_mutex_unlock(&matchMutex);
    return 1;
}

int VisionMatch::getVmarkInfoList(QMap<int, VMarkInfo> &vmarkInfoOut)
{
    pthread_mutex_lock(&matchMutex);
    vmarkInfoOut=vmarkPosition;
    pthread_mutex_unlock(&matchMutex);
    return 1;
}

int VisionMatch::updateVMarkFile(QString vmarkFileIn)
{
    QMap<int, VMarkInfo> tmpPositionOut;
    int tmpReturn=readVMarkFile(vmarkFileIn,tmpPositionOut );
    pthread_mutex_lock(&matchMutex);
    vmarkPosition=tmpPositionOut;
    pthread_mutex_unlock(&matchMutex);
    return tmpReturn;
}

int VisionMatch::createVMark(VMarkInfo infoIn, QString vmarkFileIn)
{
    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    QString infoFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number(1)+"/map/";
    infoFile+=vmarkFileIn;
    DomParser xmlParser;

    bool ok = xmlParser.openXml(infoFile, QIODevice::ReadWrite );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, VisionMatch::readVMarkFile open file error！";
       return -1;
    }

    QDomNodeList  nodeList;

    nodeList = xmlParser.getRootDomElement().childNodes();

    VMarkInfo tmp_markInfo;
    //判断index是否已经存在，如果已经存在，则修改
    bool isNodeExist=false;
    for(int i=0;i<nodeList.size();i++)
    {
        //节点ｒｅａｄ数据
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "vindex", ok), tmp_markInfo.vindex  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        if(tmp_markInfo.vindex==infoIn.vindex)
        {
            isNodeExist=true;
            xmlParser.writeXml( xmlParser.findSubNode(nodeList.at(i), "comment", ok), QString::fromStdString(infoIn.comment)  );
            if( !ok )
            {
                qDebug()<<"error, node error！";
                return -1;
            }

            xmlParser.writeXml( xmlParser.findSubNode(nodeList.at(i), "vmarkType", ok), infoIn.vmarkType  );
            if( !ok )
            {
                qDebug()<<"error, node error！";
                return -1;
            }
            xmlParser.writeXml( xmlParser.findSubNode(nodeList.at(i), "mapName", ok), QString::fromStdString(infoIn.mapName)  );
            if( !ok )
            {
                qDebug()<<"error, node error！";
                return -1;
            }
            xmlParser.writeXml( xmlParser.findSubNode(nodeList.at(i), "pose_x", ok), infoIn.pose_x  );
            if( !ok )
            {
                qDebug()<<"error, node error！";
                return -1;
            }
            xmlParser.writeXml( xmlParser.findSubNode(nodeList.at(i), "pose_y", ok), infoIn.pose_y  );
            if( !ok )
            {
                qDebug()<<"error, node error！";
                return -1;
            }
            xmlParser.writeXml( xmlParser.findSubNode(nodeList.at(i), "pose_r", ok), infoIn.pose_r  );
            if( !ok )
            {
                qDebug()<<"error, node error！";
                return -1;
            }
        }


    }

    if(false==isNodeExist)
    {
        QDomNode tmp_rootNode=xmlParser.getRootDomElement();
        xmlParser.addNode(tmp_rootNode,"OneVMark","");
        QDomNode  recordNode;
        QDomNodeList devNodes = xmlParser.getRootDomElement().childNodes();
        recordNode=devNodes.at(devNodes.size()-1);
        xmlParser.addNode(recordNode,"comment", QString::fromStdString(infoIn.comment) );
        xmlParser.addNode(recordNode,"vmarkType", infoIn.vmarkType  );
        xmlParser.addNode(recordNode,"mapName", QString::fromStdString(infoIn.mapName));
        xmlParser.addNode(recordNode,"vindex", infoIn.vindex);
        xmlParser.addNode(recordNode,"pose_x", infoIn.pose_x);
        xmlParser.addNode(recordNode,"pose_y", infoIn.pose_y);
        xmlParser.addNode(recordNode,"pose_r", infoIn.pose_r  );
    }

    xmlParser.closeXml(true);

    return 1;
}

int VisionMatch::detectVMark_baseMap(double min_angle, double max_angle, double max_distance, double allow_vDistance,
                                     double allow_vAngle, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    #ifdef D_USE_OPENCV
    vector<double> tmpx,tmpy,tmpz;
    int tmpLineCount,tmpVmarkCount;
    lineMatch->detectVMark_mat(*baseMap,min_angle/180.0*M_PI,max_angle/180.0*M_PI,
                                          max_distance/mPerPix,allow_vDistance/mPerPix,allow_vAngle/180.0*M_PI,tmpx,tmpy,tmpz
                               ,tmpLineCount,tmpVmarkCount);
    matchDebugInfo.detectedLineCount=tmpLineCount;
    matchDebugInfo.detectedVmarkCount=tmpVmarkCount;
    matchDebugInfo.detectedVmarkResultCount=tmpx.size();
    translateBaseMapImagePointToWorldPoint(tmpx,tmpy,tmpz,x_out,y_out,r_out);
    #endif
    return 1;
}

int VisionMatch::detectVMark_baseMap(vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{

    #ifdef D_USE_OPENCV
    pthread_mutex_lock(&matchMutex);
    if(false==vmark_detect_Parameter.contains(currentVmarkType))
    {
        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8234, robotId ,currentVmarkType);
        pthread_mutex_unlock(&matchMutex);
        return -1;
    }
    int tmpLineCount,tmpVmarkCount;
    int tmpReturn = lineMatch->detectVMark_mat(*baseMap,vmark_detect_Parameter[currentVmarkType].vmark_min_angle/180.0*M_PI,
                                      vmark_detect_Parameter[currentVmarkType].vmark_max_angle/180.0*M_PI,
                                      vmark_detect_Parameter[currentVmarkType].vmark_max_cross_distance/mPerPix,
                                      vmark_detect_Parameter[currentVmarkType].vmark_allow_vDistance/mPerPix,
                                      vmark_detect_Parameter[currentVmarkType].vmark_allow_vAngle/180.0*M_PI,x_out,y_out,r_out,
                                               tmpLineCount,tmpVmarkCount);
    matchDebugInfo.detectedLineCount=tmpLineCount;
    matchDebugInfo.detectedVmarkCount=tmpVmarkCount;
    matchDebugInfo.detectedVmarkResultCount=x_out.size();
    pthread_mutex_unlock(&matchMutex);
    return tmpReturn;
#endif
}


//int VisionMatch::calculateVMarkPosition_byLaser(double baseLink_x, double baseLink_y, double baseLink_r,
//                                                vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
//{
//    x_out.clear();
//    y_out.clear();
//    r_out.clear();

//    pthread_mutex_lock(&matchMutex);
//    if(false==vmark_detect_Parameter.contains(currentVmarkType))
//    {
//        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8234, robotId ,currentVmarkType);
//         pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    vector<double> tmpX, tmpY, tmpR;
//    int tmpLineCount,tmpVmarkCount;
//    lineMatch->detectVMark_mat(*localCompareMap,vmark_detect_Parameter[currentVmarkType].vmark_min_angle/180.0*M_PI,
//                               vmark_detect_Parameter[currentVmarkType].vmark_max_angle/180.0*M_PI,
//                               vmark_detect_Parameter[currentVmarkType].vmark_max_cross_distance/mPerPix,
//                               vmark_detect_Parameter[currentVmarkType].vmark_allow_vDistance/mPerPix,
//                               vmark_detect_Parameter[currentVmarkType].vmark_allow_vAngle/180.0*M_PI,tmpX, tmpY, tmpR,
//                               tmpLineCount,tmpVmarkCount);
//    matchDebugInfo.detectedLineCount=tmpLineCount;
//    matchDebugInfo.detectedVmarkCount=tmpVmarkCount;
//    matchDebugInfo.detectedVmarkResultCount=tmpX.size();
//    if(tmpX.size()==0)
//    {
//        qDebug()<<"error,VisionMatch calculateMatch_vmark";
////        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8231, robotId );
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    else if(tmpX.size()>1)
//    {
//        qDebug()<<"error,VisionMatch calculateMatch_vmark";
////        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8232, robotId, tmpX.size());
//        pthread_mutex_unlock(&matchMutex);
//        return -2;
//    }
//    double xOut, yOut,rOut;
//    int tmpReturn=translate_vmark_in_localCompareMap_2_vmark_in_world(tmpX[0], tmpY[0], tmpR[0],
//            baseLink_x,baseLink_y,baseLink_r,xOut, yOut,rOut );
//    if(1==tmpReturn)
//    {
//        x_out.resize(1);
//        y_out.resize(1);
//        r_out.resize(1);
//        x_out[0]=xOut;
//        y_out[0]=yOut;
//        r_out[0]=rOut;
//    }
//    else
//    {

//    }

//    pthread_mutex_unlock(&matchMutex);
//    return 1;
//}


//int VisionMatch::detectVMark_laser(int vmark_index, vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
//{
//    x_out.clear();
//    y_out.clear();
//    r_out.clear();

//    pthread_mutex_lock(&matchMutex);
//    if(false==vmark_detect_Parameter.contains(currentVmarkType))
//    {
//        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8234, robotId ,currentVmarkType);
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    vector<double> tmpX, tmpY, tmpR;
//    int tmpLineCount,tmpVmarkCount;
//    lineMatch->detectVMark_mat(*localCompareMap,vmark_detect_Parameter[currentVmarkType].vmark_min_angle/180.0*M_PI,
//                               vmark_detect_Parameter[currentVmarkType].vmark_max_angle/180.0*M_PI,
//                               vmark_detect_Parameter[currentVmarkType].vmark_max_cross_distance/mPerPix,
//                               vmark_detect_Parameter[currentVmarkType].vmark_allow_vDistance/mPerPix,
//                               vmark_detect_Parameter[currentVmarkType].vmark_allow_vAngle/180.0*M_PI,tmpX, tmpY, tmpR
//                               ,tmpLineCount,tmpVmarkCount);
//    matchDebugInfo.detectedLineCount=tmpLineCount;
//    matchDebugInfo.detectedVmarkCount=tmpVmarkCount;
//    matchDebugInfo.detectedVmarkResultCount=tmpX.size();
//    if(tmpX.size()==0)
//    {
//        qDebug()<<"error,VisionMatch calculateMatch_vmark";
////        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8231, robotId );
//       pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    else if(tmpX.size()>1)
//    {
//        qDebug()<<"error,VisionMatch calculateMatch_vmark";
////        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8232, robotId, tmpX.size());
//        pthread_mutex_unlock(&matchMutex);
//        return -2;
//    }
//    double xOut, yOut,rOut;
//    int tmpReturn=translate_vmark_in_localCompareMap_2_baselink_in_world(tmpX[0], tmpY[0], tmpR[0],currentVmarkIndex,xOut, yOut,rOut );
//    if(1==tmpReturn)
//    {
//        x_out.resize(1);
//        y_out.resize(1);
//        r_out.resize(1);
//        x_out[0]=xOut;
//        y_out[0]=yOut;
//        r_out[0]=rOut;
//        matchDebugInfo.isMatchSucess=true;
//    }
//    else
//    {
//        matchDebugInfo.isMatchSucess=false;
//    }

//    pthread_mutex_unlock(&matchMutex);
//    return 1;
//}

//int VisionMatch::detectVMark_laser(vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
//{

//    //时间分析
//    struct timespec startTime,endTime;
//    clock_gettime(CLOCK_MONOTONIC, &startTime);

//    int tmpReturn = detectVMark_laser(currentVmarkIndex ,x_out,y_out,r_out);
//    if(1!=tmpReturn)
//    {
//        matchDebugInfo.isMatchSucess=false;
//    }

//    clock_gettime(CLOCK_MONOTONIC, &endTime);
//    currentConsumed=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;
//    currentConsumed=currentConsumed/1000.0;
//    if(currentConsumed<minConsumed)
//    {
//        minConsumed=currentConsumed;
//    }
//    if(currentConsumed>maxConsumed)
//    {
//        maxConsumed=currentConsumed;
//    }
//    matchDebugInfo.minConsumed=minConsumed;
//    matchDebugInfo.currentConsumed=currentConsumed;
//    matchDebugInfo.maxConsumed=maxConsumed;

//    return tmpReturn;
//}

int VisionMatch::setMatchCalculateParameter(MatchCalculationParameter paraIn)
{
    pthread_mutex_lock(&matchMutex);
    calculationParemeter=paraIn;
    pthread_mutex_unlock(&matchMutex);
    return 1;

}



int VisionMatch::getMatchDebugInfo(VisionMatchDebugInfo &infoOut)
{
    matchDebugInfo.detectFailedType=(int)detectFailedType;
    matchDebugInfo.currentVmarkType=currentVmarkType;
    infoOut=matchDebugInfo;
    return 1;
}



//int VisionMatch::setLaserToVehicleInfo(LaserScanDeviceInfo infoIn)
//{
//    laserScanDeviceInfo=infoIn;

//    //initial frame
//    KDL::Rotation tmpRotation;
//    tmpRotation=tmpRotation.RPY(infoIn.pose_roll,infoIn.pose_pitch,infoIn.pose_yaw);
//    KDL::Vector tmpVector(infoIn.pose_x,infoIn.pose_y,infoIn.pose_z);
//    KDL::Frame tmpFrame(tmpRotation,tmpVector);
//    *laserToVehicleFrame=tmpFrame.Inverse();

//    return 1;
//}




//int VisionMatch::loadBaseMap(QString mapPath, double mPerPixIn, double x_offset, double y_offset, double r_offset)
//{

//    pthread_mutex_lock(&matchMutex);
//    mPerPix=mPerPixIn;//for debug
//    baseMap_originX=x_offset;
//    baseMap_originY=y_offset;
//    baseMap_originR=r_offset;
//    baseMap->release();
//    baseMap_originSource->release();
//    *baseMap_originSource = imread( mapPath.toStdString() );

//    if( !baseMap_originSource->data  )
//    {
//        std::cout<< " Error ,,,VisionMatch::loadBaseMap " << std::endl;
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
////    imagine_threshold_process(baseMap);
//    processPictureForVMark(*baseMap_originSource,*baseMap);
//    pthread_mutex_unlock(&matchMutex);
////    setBaseMapRegion_pix(baseCompare_originX,baseCompare_originY,baseCompare_width,baseCompare_height);

//    return 1;

//}

int VisionMatch::setBaseMapRegion_m(double originX, double originY, double width, double height)
{
//    return setBaseMapRegion_pix(originX/mPerPix,originY/mPerPix,width/mPerPix,height/mPerPix);
}

//int VisionMatch::setBaseMapRegion_pix(double originX, double originY, double width, double height)
//{
//    pthread_mutex_lock(&matchMutex);

//    if( 0 > width ||  0 > height )
//    {
//        qDebug()<<"error, setBaseMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    if(0 > originX && 0 >= originX+width  )
//    {
//        qDebug()<<"error, setBaseMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    else if(0 > originX && 0 < originX+width  )
//    {
//        originX=0;
//    }

//    if(0 > originY && 0 >= originY+height  )
//    {
//        qDebug()<<"error, setBaseMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    else if(0 > originY && 0 < originY+height  )
//    {
//        originY=0;
//    }

//    if(originX>baseMap->cols ||originY>baseMap->rows )
//    {
//        qDebug()<<"error, setBaseMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -2;
//    }

//    if( originX + width > baseMap->cols )
//    {
//        width=baseMap->cols-originX-1;
//    }
//    if(originY + height > baseMap->rows)
//    {
//        height=baseMap->rows-originY-1;
//    }
//    Rect rect(originX, originY, width, height);

//    *baseCompareMap = (*baseMap)(rect);

//    //补偿全局地图的原点偏移
//    baseCompare_originX=originX+baseMap_originX;
//    baseCompare_originY=originY+baseMap_originY;
//    baseCompare_width=width;
//    baseCompare_height=height;
//    pthread_mutex_unlock(&matchMutex);
//    return 1;

//}

//int VisionMatch::setLaserPosInLocalMap_pix(double laserX_in, double laserY_in, double laserR_in)
//{
//    pthread_mutex_lock(&matchMutex);
//    localMap_originX=laserX_in;
//    localMap_originY=laserY_in;
//    localMap_originR=laserR_in;
//    pthread_mutex_unlock(&matchMutex);
//    return 1;
//}

//int VisionMatch::loadLocalMap(std::vector<double> pointX, std::vector<double> pointY)
//{
////    Mat M(pointX.size(),pointY.size(), CV_8UC1, Scalar(0));
////    	resize(srcImage, dstImage, Size(50, 50), 0, 0, 3);
////        size_t tmpSize=1;
////        localMap->resize(tmpSize, Scalar(0));

//    pthread_mutex_lock(&matchMutex);

//    std::vector<int> int_x,int_y;
//    int_x.resize(pointX.size());
//    int_y.resize(pointX.size());


//    //转换成ｐｉｘ单位
//    for(int i=0;i<pointX.size();i++)
//    {
//        int_x[i]=pointX[i]/mPerPix;//ｄｏｕｂｌｅ转int误差
//        int_y[i]=pointY[i]/mPerPix;
//    }

//    //框选有效点的区域
//    int tmpMinX=D_MAX_INTEGE;//-2^31+1
//    int tmpMinY=D_MAX_INTEGE;
//    int tmpMaxX=-D_MAX_INTEGE;
//    int tmpMaxY=-D_MAX_INTEGE;
//    int originMapWidth,originMapHeight;
//    for(int i=0;i<int_x.size();i++)
//    {
//        if(int_x[i]<tmpMinX)
//        {
//           tmpMinX= int_x[i];
//        }
//        if(int_x[i]>tmpMaxX)
//        {
//           tmpMaxX= int_x[i];
//        }
//        if(int_y[i]<tmpMinY)
//        {
//           tmpMinY= int_y[i];
//        }
//        if(int_y[i]>tmpMaxY)
//        {
//           tmpMaxY= int_y[i];
//        }
//    }
//    originMapWidth=tmpMaxX-tmpMinX+2;//预留１位，防止ｄｏｕｂｌｅ转int误差
//    originMapHeight=tmpMaxY-tmpMinY+2;
//    pthread_mutex_unlock(&matchMutex);

//    //setOrigin
////    localMap_originX=-tmpMinX;
////    localMap_originY=-tmpMinY;//why?
//    localMap_originX=-tmpMinY;
//    localMap_originY=-tmpMinX;//old
//    localMap_originR=0;

//    pthread_mutex_lock(&matchMutex);

//    //LOAD MAP
//    localMap->release();
//    *localMap=Mat(originMapWidth,originMapHeight, CV_8UC1, Scalar(0));
//    for(int i=0;i<int_x.size();i++)
//    {
//        localMap->at<uchar>(int_x[i]-tmpMinX,int_y[i]-tmpMinY)=255;
//    }
////    qDebug()<<"11111-----------------------------------------localMap_originX"<<localMap_originX<<"localMap_originY"
////           <<localMap_originY;

//    pthread_mutex_unlock(&matchMutex);

//    return 1;
//}

//int VisionMatch::loadLocalMap(QString mapPath)
//{
//    pthread_mutex_lock(&matchMutex);
//    localMap->release();
//    *localMap = imread( mapPath.toStdString() );

//    if( !localMap->data  )
//    {
//        std::cout<< " --(!) Error reading LocalMap " << std::endl;
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }

////    setLocalMapRegion_pix(localCompare_originX,localCompare_originY,localCompare_width,localCompare_height);
//    return 1;
//}

//int VisionMatch::setLocalMapRegion_pix(double originX, double originY, double width, double height)
//{
//    pthread_mutex_lock(&matchMutex);
////    qDebug()<<"3333***********************************************localMap_originX"<<localMap_originX<<"localMap_originY"
////           <<localMap_originY<<"  originX"<<originX<<"originY"<<originY;
//    if(localMap->empty())
//    {
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }

//    if( 0 > width ||  0 > height )
//    {
//        qDebug()<<"error, setLocalMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    if(0 > originX && 0 >= originX+width  )
//    {
//        qDebug()<<"error, setLocalMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    else if(0 > originX && 0 < originX+width  )
//    {
//        originX=0;
//        width=originX+width;
//    }

//    if(0 > originY && 0 >= originY+height  )
//    {
//        qDebug()<<"error, setLocalMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -1;
//    }
//    else if(0 > originY && 0 < originY+height  )
//    {
//        originY=0;
//        height=originY+height;
//    }

//    if(originX>localMap->cols ||originY>localMap->rows )
//    {
//        qDebug()<<"error, setLocalMapRegion";
//        pthread_mutex_unlock(&matchMutex);
//        return -2;
//    }

//    if( originX + width > localMap->cols )
//    {
//        width=localMap->cols-originX-1;
//    }
//    if(originY + height > localMap->rows)
//    {
//        height=localMap->rows-originY-1;
//    }
//    Rect rect(originX, originY, width, height);

//    *localCompareMap = (*localMap)(rect);

//    localCompare_originX=localMap_originX-originX;
//    localCompare_originY=localMap_originY-originY;
////    localCompare_originX=localMap_originY-originY;
////    localCompare_originY=localMap_originX-originX;//错误
//    localCompare_width=width;
//    localCompare_height=height;
//    pthread_mutex_unlock(&matchMutex);
//    return 1;
//}

int VisionMatch::setLocalMapRegion_m(double originX, double originY, double width, double height)
{

    double tmp_xnew=originX/mPerPix+localMap_originX;
    double tmp_ynew=originY/mPerPix+localMap_originY;
//    qDebug()<<"22222////////////////////////////////localMap_originX"<<localMap_originX<<"localMap_originY"
//           <<localMap_originY<<"  originX"<<originY<<"originY"<<originY <<"tmp_xnew"<<tmp_xnew
//          <<"tmp_ynew"<<tmp_ynew;
//    return setLocalMapRegion_pix(tmp_xnew,tmp_ynew,width/mPerPix,height/mPerPix);
}

int VisionMatch::calculateMatch()
{
    pthread_mutex_lock(&matchMutex);
    //for debug
//    matchDebugInfo.baseCompareCols=baseCompareMap->cols;
//    matchDebugInfo.baseCompareRows=baseCompareMap->rows;
//    matchDebugInfo.localCompareCols=localCompareMap->cols;
//    matchDebugInfo.localCompareRows=localCompareMap->rows;
//    matchDebugInfo.keyPointFactor=calculationParemeter.keyPointFactor;
//    matchDebugInfo.matchFactor=calculationParemeter.matchFactor;
//    matchDebugInfo.minScale=calculationParemeter.minScale;
//    matchDebugInfo.maxScale=calculationParemeter.maxScale;



//    int tmpReturn;
//    switch(calculationParemeter.method)
//    {
//    case E_MATCH_CALCULATE_METHOD_SURF:
//    {
//        tmpReturn= calculateMatch_surf();
//        break;
//    }
//    case E_MATCH_CALCULATE_METHOD_SURF_LOWE:
//    {
//        tmpReturn= calculateMatch_surfLowe();
//        break;
//    }
//    case E_MATCH_CALCULATE_METHOD_SIFT:
//    {
//        tmpReturn= calculateMatch_sift();
//        break;
//    }
//    case E_MATCH_CALCULATE_METHOD_SIFT_LOWE:
//    {
//        tmpReturn= calculateMatch_siftLowe();
//        break;
//    }
//    case E_MATCH_CALCULATE_METHOD_ORB:
//    {
//        tmpReturn= calculateMatch_orb();
//        break;
//    }
//    case E_MATCH_CALCULATE_METHOD_FAST:
//    {
//        tmpReturn= calculateMatch_fast();
//        break;
//    }
//    case E_MATCH_CALCULATE_METHOD_HARRIS:
//    {
//        tmpReturn= calculateMatch_harris();
//        break;
//    }
//    case E_MATCH_CALCULATE_METHOD_VMARK:
//    {
//        tmpReturn= calculateMatch_vmark();
//        break;
//    }
//    default:
//    {
//        tmpReturn=-1;
//        qDebug()<<"unknow method =="<<calculationParemeter.method;
//        isMatchSucess=false;
//    }
//    }

//    matchDebugInfo.isMatchSucess=isMatchSucess;

//    pthread_mutex_unlock(&matchMutex);
//    return tmpReturn;
}

int VisionMatch::calculateMatch(double &xOut, double &yOut, double &rOut, bool &isCalculateSucess_out)
{
    int tmpReturn;
    tmpReturn= calculateMatch();
    getLaserPosition_m(xOut,yOut,rOut,isCalculateSucess_out);//hualei to do
    return tmpReturn;
}

//int VisionMatch::calculateMatch_surf()
//{
//    //-- Step 1: Detect the keypoints using SURF Detector
//    int minHessian = 400;

//    SurfFeatureDetector detector( minHessian );

//    std::vector<KeyPoint> keypoints_1, keypoints_2;

//    detector.detect( *localCompareMap, keypoints_1 );
//    detector.detect( *baseCompareMap, keypoints_2 );

//    //-- Step 2: Calculate descriptors (feature vectors)
//    SurfDescriptorExtractor extractor;

//    Mat descriptors_1, descriptors_2;

//    extractor.compute( *localCompareMap, keypoints_1, descriptors_1 );
//    extractor.compute( *baseCompareMap, keypoints_2, descriptors_2 );

//    //-- Step 3: Matching descriptor vectors using FLANN matcher
//    FlannBasedMatcher matcher;//not good
//    std::vector< DMatch > matches;
//    matcher.match( descriptors_1, descriptors_2, matches );

//    double max_dist = 0; double min_dist = 100;

//    //-- Quick calculation of max and min distances between keypoints
//    for( int i = 0; i < descriptors_1.rows; i++ )
//    { double dist = matches[i].distance;
//      if( dist < min_dist ) min_dist = dist;
//      if( dist > max_dist ) max_dist = dist;
//    }

//    printf("-- Max dist : %f \n", max_dist );
//    printf("-- Min dist : %f \n", min_dist );

//    //-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )
//    //-- PS.- radiusMatch can also be used here.
//    std::vector< DMatch > good_matches;

//     for( int i = 0; i < descriptors_1.rows; i++ )
//     { if( matches[i].distance < 2*min_dist )
//       { good_matches.push_back( matches[i]); }
//     }

////     for( int i = 0; i < descriptors_1.rows; i++ )
////     {
////       { good_matches.push_back( matches[i]); }
////     }

////    for( int i = 0; i < descriptors_1.rows; i++ )
////    { if( matches[i].distance ==min_dist )
////      { good_matches.push_back( matches[i]); }
////    }

//    //-- Draw only "good" matches
//    Mat img_matches;
//    drawMatches( *localCompareMap, keypoints_1, *baseCompareMap, keypoints_2,
//                 good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
//                 vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );


//    //-- Localize the object
//    std::vector<Point2f> obj;
//    std::vector<Point2f> scene;

//    for( int i = 0; i < good_matches.size(); i++ )
//    {
//      //-- Get the keypoints from the good matches
//      obj.push_back( keypoints_1[ good_matches[i].queryIdx ].pt );
//      scene.push_back( keypoints_2[ good_matches[i].trainIdx ].pt );
//    }

//    Mat transMat = findHomography( obj, scene, CV_RANSAC );
//    cout<<"transMat="<<transMat<<endl;
//    local_matchX=transMat.at<double>(0,2);
//    local_matchY=transMat.at<double>(1,2);
////    int tmpSize=transMat.elemSize1();

//    //-- Show detected matches
//    imshow( "Good Matches", img_matches );

////    for( int i = 0; i < good_matches.size(); i++ )
////    { printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx ); }

//    return 1;
//}

//int VisionMatch::calculateMatch_sift()
//{
//    //-- Step 1: Detect the keypoints using SURF Detector
//    int minHessian = 400;

//    SiftFeatureDetector detector( minHessian );

//    std::vector<KeyPoint> keypoints_1, keypoints_2;

//    detector.detect( *localCompareMap, keypoints_1 );
//    detector.detect( *baseCompareMap, keypoints_2 );

//    //-- Step 2: Calculate descriptors (feature vectors)
//    SiftFeatureDetector extractor;

//    Mat descriptors_1, descriptors_2;

//    extractor.compute( *localCompareMap, keypoints_1, descriptors_1 );
//    extractor.compute( *baseCompareMap, keypoints_2, descriptors_2 );

//    //-- Step 3: Matching descriptor vectors using FLANN matcher
//    FlannBasedMatcher matcher;//not good
//    std::vector< DMatch > matches;
//    matcher.match( descriptors_1, descriptors_2, matches );

//    double max_dist = 0; double min_dist = 100;

//    //-- Quick calculation of max and min distances between keypoints
//    for( int i = 0; i < descriptors_1.rows; i++ )
//    { double dist = matches[i].distance;
//      if( dist < min_dist ) min_dist = dist;
//      if( dist > max_dist ) max_dist = dist;
//    }

//    printf("-- Max dist : %f \n", max_dist );
//    printf("-- Min dist : %f \n", min_dist );

//    //-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )
//    //-- PS.- radiusMatch can also be used here.
//    std::vector< DMatch > good_matches;

//     for( int i = 0; i < descriptors_1.rows; i++ )
//     { if( matches[i].distance < 2*min_dist )
//       { good_matches.push_back( matches[i]); }
//     }

////     for( int i = 0; i < descriptors_1.rows; i++ )
////     {
////       { good_matches.push_back( matches[i]); }
////     }

////    for( int i = 0; i < descriptors_1.rows; i++ )
////    { if( matches[i].distance ==min_dist )
////      { good_matches.push_back( matches[i]); }
////    }

//    //-- Draw only "good" matches
//    Mat img_matches;
//    drawMatches( *localCompareMap, keypoints_1, *baseCompareMap, keypoints_2,
//                 good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
//                 vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );


//    //-- Localize the object
//    std::vector<Point2f> obj;
//    std::vector<Point2f> scene;

//    for( int i = 0; i < good_matches.size(); i++ )
//    {
//      //-- Get the keypoints from the good matches
//      obj.push_back( keypoints_1[ good_matches[i].queryIdx ].pt );
//      scene.push_back( keypoints_2[ good_matches[i].trainIdx ].pt );
//    }

//    Mat transMat = findHomography( obj, scene, CV_RANSAC );
//    cout<<"transMat="<<transMat<<endl;
//    local_matchX=transMat.at<double>(0,2);
//    local_matchY=transMat.at<double>(1,2);
////    int tmpSize=transMat.elemSize1();

//    //-- Show detected matches
//    imshow( "Good Matches", img_matches );

////    for( int i = 0; i < good_matches.size(); i++ )
////    { printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx ); }

//    return 1;
//}

//int VisionMatch::calculateMatch_siftLowe()
//{

//    int keyPointFactor=calculationParemeter.keyPointFactor; // 海塞矩阵阈值，在这里调整精度，值越大点越少，越精准
//    float matchFactor=calculationParemeter.matchFactor;//越小点越少，精度越好
//    float minScale=calculationParemeter.minScale;
//    float maxScale=calculationParemeter.maxScale;
////    float minScale=-100;
////    float maxScale=100;

//    //提取特征点
////    GoodFeaturesToTrackDetector siftDetector(keyPointFactor);  //ｈａｒｒｉｓ :最大点数,值越大，点越多
//    OrbFeatureDetector siftDetector(keyPointFactor); //对激光好，对地图不好，丢失线条点（值要调到２０００） // 在这里调整精度，值越小点越少，越精准
////    FastFeatureDetector siftDetector(keyPointFactor);//适合激光
////    SurfFeatureDetector siftDetector(keyPointFactor);//不适合激光
////        SiftFeatureDetector siftDetector(keyPointFactor);//不适合激光  // 海塞矩阵阈值，在这里调整精度，值越大点越少，越精准
//        vector<KeyPoint> keyPoint1, keyPoint2;
//        siftDetector.detect(*baseCompareMap, keyPoint1);
//        siftDetector.detect(*localCompareMap, keyPoint2);

//        //for debug
//        matchDebugInfo.baseKeyPointNum=keyPoint1.size();
//        matchDebugInfo.localKeyPointNum=keyPoint2.size();

//        if(keyPoint1.size()<4 ||keyPoint2.size()<4  )
//        {
//            cout<<"keyPoint1.size()<4 ||keyPoint2.size()<4"<<endl;
//            isMatchSucess=false;
//            return -1;
//        }

//        //特征点描述，为下边的特征点匹配做准备
//        SiftDescriptorExtractor SiftDescriptor;
//        Mat imageDesc1, imageDesc2;
//        SiftDescriptor.compute(*baseCompareMap, keyPoint1, imageDesc1);
//        SiftDescriptor.compute(*localCompareMap, keyPoint2, imageDesc2);

//        flann::KDTreeIndexParams indexParams;
//        flann::SearchParams searchParams(200);

//        FlannBasedMatcher matcher(&indexParams,&searchParams);
////        一个参数是IndexParams，对于SIFT和SURF，可以传入参数index_params=dict(algorithm=FLANN_INDEX_KDTREE,
////        trees=5)。
////         对于ORB，可以传入参数index_params=dict(algorithm=FLANN_INDEX_LSH, table_number=6, key_size=12,
////        multi_probe_level=1)。
////        第二个参数是SearchParams，可以传入参数search_params=dict(checks=100)，它来指定递归遍历的次数，
////        值越高结果越准确，但是消耗的时间也越多。
//        vector<vector<DMatch> > matchePoints;
//        vector<DMatch> goodMatchePoints;

//        vector<Mat> train_desc(1, imageDesc1);
//        matcher.add(train_desc);
//        matcher.train();

//        matcher.knnMatch(imageDesc2, matchePoints, 2);
//        cout << "total match points: " << matchePoints.size() << endl;

//        //for debug
//        matchDebugInfo.totalMatchNum=matchePoints.size();

//        // Lowe's algorithm,获取优秀匹配点
//        for (int i = 0; i < matchePoints.size(); i++)
//        {
//            if (matchePoints[i][0].distance < matchFactor* matchePoints[i][1].distance)
//            {
//                goodMatchePoints.push_back(matchePoints[i][0]);
//            }
//        }

//        //for debug
//        matchDebugInfo.goodMatchNum=goodMatchePoints.size();

//        Mat first_match;
//        if(matchePoints.size()>0)
//        {
//            drawMatches(*localCompareMap, keyPoint2, *baseCompareMap, keyPoint1, goodMatchePoints, first_match);
//            imshow("first_match ", first_match);
//    //        imwrite("first_match.jpg", first_match);
//        }

//        //-- Localize the object
//        std::vector<Point2f> obj;
//        std::vector<Point2f> scene;

//        if(goodMatchePoints.size()<4)
//        {
//            cout<<"error,goodMatchePoints.size()<4,size="<<goodMatchePoints.size()<<endl;
//            isMatchSucess=false;
//            return -1;
//        }

//        for( int i = 0; i < goodMatchePoints.size(); i++ )
//        {
//          //-- Get the keypoints from the good matches
//          obj.push_back( keyPoint2[ goodMatchePoints[i].queryIdx ].pt );
//          scene.push_back( keyPoint1[ goodMatchePoints[i].trainIdx ].pt );
//        }

//        *transMat = findHomography( obj, scene, CV_RANSAC );
//        cout<<"transMat="<<*transMat<<endl;



//        if(transMat->at<double>(0,0)<minScale ||transMat->at<double>(0,0)>maxScale)
//        {
//            //for debug
//            matchDebugInfo.matchScale=transMat->at<double>(0,0);
//            cout<<"error,scale error"<<endl;
//            isMatchSucess=false;
//            return -1;
//        }

//        if(transMat->at<double>(1,1)<minScale ||transMat->at<double>(1,1)>maxScale)
//        {
//            //for debug
//            matchDebugInfo.matchScale=transMat->at<double>(1,1);
//            cout<<"error,scale error"<<endl;
//            isMatchSucess=false;
//            return -1;
//        }
//        transPointToBaseMap();

//        isMatchSucess=true;
//        return 1;
//}

//int VisionMatch::calculateMatch_siftLowe()
//{

//    int keyPointFactor=calculationParemeter.keyPointFactor; // 海塞矩阵阈值，在这里调整精度，值越大点越少，越精准
//    float matchFactor=calculationParemeter.matchFactor;//越小点越少，精度越好
//    float minScale=calculationParemeter.minScale;
//    float maxScale=calculationParemeter.maxScale;
////    float minScale=-100;
////    float maxScale=100;

//    //提取特征点
////    GoodFeaturesToTrackDetector siftDetector(keyPointFactor);  //ｈａｒｒｉｓ :最大点数,值越大，点越多
////    OrbFeatureDetector siftDetector(keyPointFactor); //对激光好，对地图不好，丢失线条点（值要调到２０００） // 在这里调整精度，值越小点越少，越精准
////    FastFeatureDetector siftDetector(keyPointFactor);//适合激光
////    SurfFeatureDetector siftDetector(keyPointFactor);//不适合激光
////        SiftFeatureDetector siftDetector(keyPointFactor);//不适合激光  // 海塞矩阵阈值，在这里调整精度，值越大点越少，越精准
////        vector<KeyPoint> keyPoint1, keyPoint2;
////        siftDetector.detect(*baseCompareMap, keyPoint1);
////        siftDetector.detect(*localCompareMap, keyPoint2);

////        //for debug
////        matchDebugInfo.baseKeyPointNum=keyPoint1.size();
////        matchDebugInfo.localKeyPointNum=keyPoint2.size();

////        if(keyPoint1.size()<4 ||keyPoint2.size()<4  )
////        {
////            cout<<"keyPoint1.size()<4 ||keyPoint2.size()<4"<<endl;
////            isMatchSucess=false;
////            return -1;
////        }

////        //特征点描述，为下边的特征点匹配做准备
////        SiftDescriptorExtractor SiftDescriptor;
////        Mat imageDesc1, imageDesc2;
////        SiftDescriptor.compute(*baseCompareMap, keyPoint1, imageDesc1);
////        SiftDescriptor.compute(*localCompareMap, keyPoint2, imageDesc2);

////        FlannBasedMatcher matcher;
////        vector<vector<DMatch> > matchePoints;
////        vector<DMatch> goodMatchePoints;

////        vector<Mat> train_desc(1, imageDesc1);
////        matcher.add(train_desc);
////        matcher.train();

////        matcher.knnMatch(imageDesc2, matchePoints, 2);
////        cout << "total match points: " << matchePoints.size() << endl;

////        //for debug
////        matchDebugInfo.totalMatchNum=matchePoints.size();

////        // Lowe's algorithm,获取优秀匹配点
////        for (int i = 0; i < matchePoints.size(); i++)
////        {
////            if (matchePoints[i][0].distance < matchFactor* matchePoints[i][1].distance)
////            {
////                goodMatchePoints.push_back(matchePoints[i][0]);
////            }
////        }

////        //for debug
////        matchDebugInfo.goodMatchNum=goodMatchePoints.size();

////        Mat first_match;
////        if(matchePoints.size()>0)
////        {
////            drawMatches(*localCompareMap, keyPoint2, *baseCompareMap, keyPoint1, goodMatchePoints, first_match);
////            imshow("first_match ", first_match);
////    //        imwrite("first_match.jpg", first_match);
////        }

////        //-- Localize the object
////        std::vector<Point2f> obj;
////        std::vector<Point2f> scene;

////        if(goodMatchePoints.size()<4)
////        {
////            cout<<"error,goodMatchePoints.size()<4,size="<<goodMatchePoints.size()<<endl;
////            isMatchSucess=false;
////            return -1;
////        }

////        for( int i = 0; i < goodMatchePoints.size(); i++ )
////        {
////          //-- Get the keypoints from the good matches
////          obj.push_back( keyPoint2[ goodMatchePoints[i].queryIdx ].pt );
////          scene.push_back( keyPoint1[ goodMatchePoints[i].trainIdx ].pt );
////        }

////        *transMat = findHomography( obj, scene, CV_RANSAC );
////        cout<<"transMat="<<*transMat<<endl;
//        //---------------------------------------------------------------
////            Mat img1_src = imread("im5.jpg",0);
////            Mat img2_src = imread("im6.jpg",0);
//            //FastFeatureDetector fast(40);
////            SurfFeatureDetector fast(2000,4);

//    //提取特征点
////    GoodFeaturesToTrackDetector siftDetector(keyPointFactor);  //ｈａｒｒｉｓ :最大点数,值越大，点越多
//    OrbFeatureDetector siftDetector(keyPointFactor); //对激光好，对地图不好，丢失线条点（值要调到２０００） // 在这里调整精度，值越小点越少，越精准
////    FastFeatureDetector siftDetector(keyPointFactor);//适合激光,keyPointFactor=2
////    SurfFeatureDetector siftDetector(keyPointFactor);//不适合激光
////        SiftFeatureDetector siftDetector(keyPointFactor);//不适合激光  // 海塞矩阵阈值，在这里调整精度，值越大点越少，越精准
////

//            vector<KeyPoint> keypoints1,keypoints2;
//            siftDetector.detect(*localCompareMap,keypoints1);
//            siftDetector.detect(*baseCompareMap,keypoints2);

//            if(keypoints1.size()<4 ||keypoints1.size()<4  )
//            {
//                cout<<"keypoints1.size()<4 ||keypoints1.size()<4"<<endl;
//                isMatchSucess=false;
//                return -1;
//            }

//            //for debug
//            matchDebugInfo.baseKeyPointNum=keypoints2.size();
//            matchDebugInfo.localKeyPointNum=keypoints1.size();
//            //drawKeypoints(img1_src,keypoints1,img1_src,Scalar(0,255,0));
//            //drawKeypoints(img2_src,keypoints2,img2_src,Scalar(0,255,0));//问题在这里！！！醉了，这里的问题，浪费了我5天，欧耶，就是整整5天，由于我想在这里看一看检测出来的特征点是啥样的，就跟父母想第一眼就立刻看到刚出生的婴儿是一个心情的。结果，后来特征匹配就几乎没有什么正确匹配，害我还以为是FREAK这个描述子有问题呢。于是就各种找问题，看论文，翻墙逛论坛。最后一个不经意间发现了。问题就是：这俩个语句是把特征点又原封不动的画到了img1_src中，也就是原图像里面，而后来我进行特征点描述的时候，就直接在画满了特征点的图片下进行描述，而不是原图！不是原图啊！是充满了特征点的图片！所以后期再进行匹配的时候，显然，各种乱匹配，就跟隔壁家小狗似的，见了猫都想干坏事。于是乎，我直接注释掉了这两句！

//            Mat descriptor1,descriptor2;
//            vector<DMatch> final_matches;
//            vector<DMatch> matches;
//            double t = (double)getTickCount();

//            FREAK extractor;
//            extractor.compute(*localCompareMap,keypoints1,descriptor1);
//            extractor.compute(*baseCompareMap,keypoints2,descriptor2);

//        BFMatcher matcher(NORM_HAMMING,true);//暴力匹配，并且进行crosscheck，就是说第二个参数选择true。
//            matcher.match(descriptor1,descriptor2,matches);
//            final_matches=matches;
//            cout<<"number of total_matches : "<<final_matches.size()<<endl;

//            //for debug
//            matchDebugInfo.totalMatchNum=final_matches.size();

//            Mat first_match;
//            if(matches.size()>0)
//            {
//                drawMatches(*localCompareMap, keypoints1, *baseCompareMap, keypoints2,
//                            matches, first_match);
//                imshow("first_match ", first_match);
//            }


//        //接下来是RANSAC剔除误匹配
//            vector<Point2f> querymatches, trainmatches;
//            vector<KeyPoint> p1,p2;
//            for(int i=0;i<final_matches.size();i++)
//            {
//                p1.push_back(keypoints1[final_matches[i].queryIdx]);
//                p2.push_back(keypoints2[final_matches[i].trainIdx]);
//            }
//            for(int i=0;i<p1.size();i++)
//            {
//                querymatches.push_back(p1[i].pt);
//                trainmatches.push_back(p2[i].pt);
//            }
////            cout<<querymatches[1]<<" and "<<trainmatches[1]<<endl;
//            vector<uchar> status;
//            if(querymatches.size()<4)
//            {
//                cout<<"error,querymatches.size()<4,size="<<querymatches.size()<<endl;
//                isMatchSucess=false;
//                return -1;
//            }
//            *transMat = findHomography(querymatches,trainmatches,status,CV_FM_RANSAC,10);
//            int index=0;
//            vector<DMatch> super_final_matches;
//            for (int i=0;i<final_matches.size();i++)
//            {
//                cout<<status[i];
//                if (status[i] != 0)
//                {
//                    super_final_matches.push_back(final_matches[i]);
//                    index++;
//                }
//            }
//            cout<<"number of inlier_matches : "<<index<<endl;
//            //for debug
//            matchDebugInfo.goodMatchNum=index;



//        //----------------------------------------------



//        if(transMat->at<double>(0,0)<minScale ||transMat->at<double>(0,0)>maxScale)
//        {
//            //for debug
//            matchDebugInfo.matchScale=transMat->at<double>(0,0);
//            cout<<"error,scale error"<<endl;
//            isMatchSucess=false;
//            return -1;
//        }

//        if(transMat->at<double>(1,1)<minScale ||transMat->at<double>(1,1)>maxScale)
//        {
//            //for debug
//            matchDebugInfo.matchScale=transMat->at<double>(1,1);
//            cout<<"error,scale error"<<endl;
//            isMatchSucess=false;
//            return -1;
//        }
//        transPointToBaseMap();

//        isMatchSucess=true;
//        return 1;
//}


int VisionMatch::transPointToBaseMap()
{
//    //-- Get the corners from the image_1 ( the object to be "detected" )
//    std::vector<Point2f> obj_corners(1);
//    obj_corners[0] = cvPoint(localCompare_originX,localCompare_originY);
//    std::vector<Point2f> scene_corners(1);

//    perspectiveTransform( obj_corners, scene_corners, *transMat);
//    cout<<"matchPointPos="<<scene_corners<<endl;
//    local_matchX=scene_corners[0].x;
//    local_matchY=scene_corners[0].y;



//    KDL::Rotation tmpRotation(transMat->at<double>(0,0),transMat->at<double>(0,1),0,
//                              transMat->at<double>(1,0),transMat->at<double>(1,1),0,
//                              0,0,1);
//    double tmpR,tmpP,tmpY;
//    tmpRotation.GetRPY(tmpR,tmpP,tmpY);
//    cout<<"tmpR="<<tmpR<<endl;
//    cout<<"tmpP="<<tmpP<<endl;
//    cout<<"tmpY="<<tmpY<<endl;

//    local_matchR=localMap_originR+ tmpY ;
//    return 1;

}

int VisionMatch::getBaseMapMatch(double &xOut, double &yOut, double &rOut)
{
    xOut=local_matchX+baseCompare_originX;
    yOut=local_matchY+baseCompare_originY;
    rOut=local_matchR;
    return 1;
}

//int VisionMatch::imagine_threshold_process(Mat *imagine)
//{
//    int threshold_value = 1;//pgm: 0x80表示未知区域，0x00表示障碍物，0xfe表示已知区域
//    int threshold_type = 0;;
//    int const max_BINARY_value = 255;

//    /// 将图片转换成灰度图片
////     cvtColor( *imagine, *imagine, CV_RGB2GRAY );


//      /* 0: 二进制阈值
//         1: 反二进制阈值
//         2: 截断阈值
//         3: 0阈值
//         4: 反0阈值
//       */

//     threshold( *imagine, *imagine, threshold_value, max_BINARY_value,threshold_type );


//}

//int VisionMatch::processPictureForVMark(const Mat &originPicture, Mat &transformedPicture)
//{
//    Mat grayImg, binImg;
//    /*检查图像是否载入*/
//    if (originPicture.empty())
//    {
//        printf("Error Loading Image...processPictureForVMark\n");
//        return -1;
//    }

//    /*转为灰度图*/
//    if (originPicture.channels() == 3){
//        cvtColor(originPicture, grayImg, CV_BGR2GRAY);
//    }
//    else if (originPicture.channels() == 2){
//        grayImg = originPicture.clone();
//    }
////    imshow("灰度图 image", grayImg);

//    /*二值化*/
//    threshold(grayImg, binImg, 100, 255, THRESH_BINARY);
////    //adaptiveThreshold(grayImg, binImg, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY, 15, -2);
////    imshow("binary image", binImg);


//    /*腐蚀*/
//    Mat element = getStructuringElement(MORPH_RECT, Size(2, 1));
//    Mat erodeImg;
//    erode(binImg, erodeImg, element);
////    imshow("erode", erodeImg);

//    transformedPicture=~erodeImg;
//    //transformedPicture=erodeImg;
////    imshow("blackBackGround", transformedPicture);
//    return 1;
//}

double VisionMatch::transformToPi(double angleIn)
{
    //    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
        int turns;

        if(0<angleIn)
        {
            turns=(angleIn+M_PI) /(2*M_PI);
        }
        else
        {
            turns=(angleIn-M_PI) /(2*M_PI);
        }

        //    angleIn=angleIn-turns*M_PI;//错误
            angleIn=angleIn-turns*M_PI*2;

        if(angleIn>=-M_PI && angleIn<=M_PI)
        {
            return angleIn;
        }
        else if(angleIn<-M_PI)
        {
            return angleIn+2*M_PI;
        }
        else if(angleIn>M_PI)
        {
            return angleIn-2*M_PI;
        }

        return angleIn;
}


//int VisionMatch::calculateMatch_orb()
//{
//    Mat image01 = imread("g2.jpg", 1);
//        Mat image02 = imread("g4.jpg", 1);
//        imshow("p2", image01);
//        imshow("p1", image02);

//        //灰度图转换
//        Mat image1, image2;
////        cvtColor(image01, image1, CV_RGB2GRAY);
////        cvtColor(image02, image2, CV_RGB2GRAY);


//        //提取特征点
//        OrbFeatureDetector OrbDetector(1000);  // 在这里调整精度，值越小点越少，越精准
//        vector<KeyPoint> keyPoint1, keyPoint2;
//        OrbDetector.detect(image1, keyPoint1);
//        OrbDetector.detect(image2, keyPoint2);

//        //特征点描述，为下边的特征点匹配做准备
//        OrbDescriptorExtractor OrbDescriptor;
//        Mat imageDesc1, imageDesc2;
//        OrbDescriptor.compute(image1, keyPoint1, imageDesc1);
//        OrbDescriptor.compute(image2, keyPoint2, imageDesc2);

//        flann::Index flannIndex(imageDesc1, flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);

//        vector<DMatch> GoodMatchePoints;

//        Mat macthIndex(imageDesc2.rows, 2, CV_32SC1), matchDistance(imageDesc2.rows, 2, CV_32FC1);
//        flannIndex.knnSearch(imageDesc2, macthIndex, matchDistance, 2, flann::SearchParams());

//        // Lowe's algorithm,获取优秀匹配点
//        for (int i = 0; i < matchDistance.rows; i++)
//        {
//            if (matchDistance.at<float>(i,0) < 0.6 * matchDistance.at<float>(i, 1))
//            {
//                DMatch dmatches(i, macthIndex.at<int>(i, 0), matchDistance.at<float>(i, 0));
//                GoodMatchePoints.push_back(dmatches);
//            }
//        }

//        Mat first_match;
//        drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
//        imshow("first_match ", first_match);
//        imwrite("first_match.jpg", first_match);
//        waitKey();
//        return 0;
//}

//int VisionMatch::calculateMatch_fast()
//{
//    Mat image01 = imread("1.jpg", 1);
//        Mat image02 = imread("2.jpg", 1);
//        imshow("p2", image01);
//        imshow("p1", image02);

//        //灰度图转换
//        Mat image1, image2;
//        cvtColor(image01, image1, CV_RGB2GRAY);
//        cvtColor(image02, image2, CV_RGB2GRAY);


//        //提取特征点
//        FastFeatureDetector Detector(50);  //阈值
//        vector<KeyPoint> keyPoint1, keyPoint2;
//        Detector.detect(image1, keyPoint1);
//        Detector.detect(image2, keyPoint2);

//        //特征点描述，为下边的特征点匹配做准备
//        SiftDescriptorExtractor   Descriptor;
//        Mat imageDesc1, imageDesc2;
//        Descriptor.compute(image1, keyPoint1, imageDesc1);
//        Descriptor.compute(image2, keyPoint2, imageDesc2);

//        BruteForceMatcher< L2<float> > matcher;
//        vector<vector<DMatch> > matchePoints;
//        vector<DMatch> GoodMatchePoints;

//        vector<Mat> train_desc(1, imageDesc1);
//        matcher.add(train_desc);
//        matcher.train();

//        matcher.knnMatch(imageDesc2, matchePoints, 2);
//        cout << "total match points: " << matchePoints.size() << endl;

//        // Lowe's algorithm,获取优秀匹配点
//        for (int i = 0; i < matchePoints.size(); i++)
//        {
//            if (matchePoints[i][0].distance < 0.6 * matchePoints[i][1].distance)
//            {
//                GoodMatchePoints.push_back(matchePoints[i][0]);
//            }
//        }

//        Mat first_match;
//        drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
//        imshow("first_match ", first_match);
//        imwrite("first_match.jpg", first_match);
//        waitKey();
//        return 0;
//}

//int VisionMatch::calculateMatch_harris()
//{
//    Mat image01 = imread("1.jpg", 1);
//        Mat image02 = imread("2.jpg", 1);
//        imshow("p2", image01);
//        imshow("p1", image02);

//        //灰度图转换
//        Mat image1, image2;
//        cvtColor(image01, image1, CV_RGB2GRAY);
//        cvtColor(image02, image2, CV_RGB2GRAY);


//        //提取特征点
//        GoodFeaturesToTrackDetector Detector(500);  //最大点数,值越大，点越多
//        vector<KeyPoint> keyPoint1, keyPoint2;
//        Detector.detect(image1, keyPoint1);
//        Detector.detect(image2, keyPoint2);

//        //特征点描述，为下边的特征点匹配做准备
//        SiftDescriptorExtractor  Descriptor;
//        Mat imageDesc1, imageDesc2;
//        Descriptor.compute(image1, keyPoint1, imageDesc1);
//        Descriptor.compute(image2, keyPoint2, imageDesc2);

//        BruteForceMatcher< L2<float> > matcher;
//        vector<vector<DMatch> > matchePoints;
//        vector<DMatch> GoodMatchePoints;

//        vector<Mat> train_desc(1, imageDesc1);
//        matcher.add(train_desc);
//        matcher.train();

//        matcher.knnMatch(imageDesc2, matchePoints, 2);
//        cout << "total match points: " << matchePoints.size() << endl;

//        // Lowe's algorithm,获取优秀匹配点
//        for (int i = 0; i < matchePoints.size(); i++)
//        {
//            if (matchePoints[i][0].distance < 0.6 * matchePoints[i][1].distance)
//            {
//                GoodMatchePoints.push_back(matchePoints[i][0]);
//            }
//        }

//        Mat first_match;
//        drawMatches(image02, keyPoint2, image01, keyPoint1, GoodMatchePoints, first_match);
//        imshow("first_match ", first_match);
//        imwrite("first_match.jpg", first_match);
//        waitKey();
//        return 0;
//}

//int VisionMatch::calculateMatch_vmark()
//{


//}


//int VisionMatch::calculateMatch_surfLowe()
//{
//    //-- Step 1: Detect the keypoints using SURF Detector
//    int minHessian = 2000;

//    SurfFeatureDetector detector( minHessian );

//    std::vector<KeyPoint> keypoints_1, keypoints_2;

//    detector.detect( *localCompareMap, keypoints_1 );
//    detector.detect( *baseCompareMap, keypoints_2 );

//    //-- Step 2: Calculate descriptors (feature vectors)
//    SurfDescriptorExtractor extractor;

//    Mat descriptors_1, descriptors_2;

//    extractor.compute( *localCompareMap, keypoints_1, descriptors_1 );
//    extractor.compute( *baseCompareMap, keypoints_2, descriptors_2 );

//    //-- Step 3: Matching descriptor vectors using FLANN matcher
//    FlannBasedMatcher matcher;
//    vector<vector<DMatch> > matchePoints;
//    vector<DMatch> goodMatchePoints;

//    vector<Mat> train_desc(1, *baseCompareMap);
//    matcher.add(train_desc);
//    matcher.train();

//    matcher.knnMatch(*localCompareMap, matchePoints, 2);
//    cout << "total match points: " << matchePoints.size() << endl;

//    // Lowe's algorithm,获取优秀匹配点
//    for (int i = 0; i < matchePoints.size(); i++)
//    {
//        if (matchePoints[i][0].distance < 0.6 * matchePoints[i][1].distance)
//        {
//            goodMatchePoints.push_back(matchePoints[i][0]);
//        }
//    }


//    //-- Draw only "good" matches
//    Mat img_matches;
//    drawMatches( *localCompareMap, keypoints_1, *baseCompareMap, keypoints_2,
//                 goodMatchePoints, img_matches, Scalar::all(-1), Scalar::all(-1),
//                 vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );


//    //-- Localize the object
//    std::vector<Point2f> obj;
//    std::vector<Point2f> scene;

//    for( int i = 0; i < goodMatchePoints.size(); i++ )
//    {
//      //-- Get the keypoints from the good matches
//      obj.push_back( keypoints_1[ goodMatchePoints[i].queryIdx ].pt );
//      scene.push_back( keypoints_2[ goodMatchePoints[i].trainIdx ].pt );
//    }

//    Mat transMat = findHomography( obj, scene, CV_RANSAC );
//    cout<<"transMat="<<transMat<<endl;
//    local_matchX=transMat.at<double>(0,2);
//    local_matchY=transMat.at<double>(1,2);
////    int tmpSize=transMat.elemSize1();

//    //-- Show detected matches
//    imshow( "Good Matches", img_matches );

////    for( int i = 0; i < good_matches.size(); i++ )
////    { printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i, good_matches[i].queryIdx, good_matches[i].trainIdx ); }

//    return 1;
//}

int VisionMatch::getLaserPosition_pix(double &xOut, double &yOut, double &rOut, bool &isCalculateSucess_out)
{
    getBaseMapMatch(xOut,yOut,rOut);
    //补偿原始偏差,暂时没有考虑旋转。
    xOut=xOut-baseMap_originX;
    yOut=yOut-baseMap_originY;
    isCalculateSucess_out=isMatchSucess;
    return 1;

}

int VisionMatch::getLaserPosition_m(double &xOut, double &yOut, double &rOut, bool &isCalculateSucess_out)
{
    int tmpReturn= getLaserPosition_pix(xOut,yOut,rOut,isCalculateSucess_out);
    xOut=xOut*mPerPix;
    yOut=yOut*mPerPix;
    return tmpReturn;

}

int VisionMatch::getVehiclePosition_m(double &xOut, double &yOut, double &rOut, bool &isCalculateSucess_out)
{
//    double tmp_xOut,  tmp_yOut,  tmp_rOut;
//    bool tmp_isCalculateSucess_out;
//    getLaserPosition_m(tmp_xOut,tmp_yOut,tmp_rOut,tmp_isCalculateSucess_out);
//    if(false==tmp_isCalculateSucess_out)
//    {
//        return -1;
//    }

//    //initial frame
//    KDL::Rotation tmpRotation;
//    tmpRotation=tmpRotation.RPY(0,0,tmp_rOut);
//    KDL::Vector tmpVector(tmp_xOut,tmp_yOut,0);
//    KDL::Frame tmpFrame(tmpRotation,tmpVector);

////    KDL::Frame desFrame=tmpFrame*(*laserToVehicleFrame);

//    xOut=desFrame.p[0];
//    yOut=desFrame.p[1];
//    double roll,pitch;
//    desFrame.M.GetRPY(roll,pitch,rOut);
//    isCalculateSucess_out=tmp_isCalculateSucess_out;
    return 1;

}

int VisionMatch::showDebugWindow(int id)
{
//    switch(id)
//    {
//    case 0:
//    {
//        if(0==pthread_mutex_trylock(&matchMutex))//
//        {
//            Mat tmpMat_baseMap=baseMap->clone();
//            Mat tmpMat_baseMap_originsource=baseMap_originSource->clone();
//            pthread_mutex_unlock(&matchMutex);
//            if( !tmpMat_baseMap.empty())
//            {
//                imshow("blackBackGround_basemap", tmpMat_baseMap);
//            }
//            lineMatch->showDebugWindow(0,tmpMat_baseMap_originsource);
//        }


//        break;
//    }
//    case 1:
//    {
//        if(0==pthread_mutex_trylock(&matchMutex))//
//        {
//            Mat tmpMat_localMap=localMap->clone();
//            Mat tmpMat_localCompareMap=localCompareMap->clone();
//            pthread_mutex_unlock(&matchMutex);
//            if( !tmpMat_localMap.empty())
//            {
//                line(tmpMat_localMap, Point(localMap_originX+0, localMap_originY+0), Point(localMap_originX+40, localMap_originY+0), Scalar(200, 255, 50), 2, 8);
//                line(tmpMat_localMap, Point(localMap_originX+0, localMap_originY+0), Point(localMap_originX+0, localMap_originY+80), Scalar(200, 255, 150), 2, 8);
//                imshow("blackBackGround_laser", tmpMat_localMap);
//            }
//            Mat three_channel;
//            if( !tmpMat_localCompareMap.empty() )
//            {

//                 three_channel= Mat::zeros(tmpMat_localCompareMap.rows,tmpMat_localCompareMap.cols,CV_8UC3);
//                 vector<Mat> channels;
//                 for (int i=0;i<3;i++)
//                 {
//                     channels.push_back(tmpMat_localCompareMap);
//                 }
//                 merge(channels,three_channel);
//                 line(three_channel, Point(localCompare_originX+0,localCompare_originY+ 0), Point(localCompare_originX+40,localCompare_originY+ 0), Scalar(200, 255, 50), 2, 8);
//                 line(three_channel, Point(localCompare_originX+0,localCompare_originY+ 0), Point(localCompare_originX+0, localCompare_originY+80), Scalar(200, 255, 150), 2, 8);
//                imshow("blackBackGround_laser_compare", three_channel);
//            }
//            lineMatch->showDebugWindow(0,three_channel);//hualei 没有加锁，可能导致ｃｏｒｅdump
//        }

//        break;
//    }
//    }

}

int VisionMatch::showBaseMap()
{
//    imshow( "baseMap", *baseMap );
}

int VisionMatch::showBaseCompareMap()
{
//    imshow( "baseCompareMap", *baseCompareMap );
}

int VisionMatch::showLocalMap()
{
//    imshow( "localMap", *localMap );
}

int VisionMatch::showLocalCampareMap()
{
//    imshow( "localCompareMap", *localCompareMap );
}

int VisionMatch::showMatchWindow()
{
//    int thickness = -1;
//    int lineType = 8;

//    Point origin;
//    origin.x = 0;
//    origin.y = 0;

//    circle( *baseCompareMap,
//             origin,
//             10,
//             Scalar( 0, 0, 255 ),
//             thickness,
//             lineType );

//    putText( *baseCompareMap, "origin", origin, 0,1, Scalar( 100, 100, 0 ));


//     Point tmpX;
//    tmpX.x = 50;
//    tmpX.y = 0;

//    circle( *baseCompareMap,
//             tmpX,
//             10,
//             Scalar( 255, 0, 0 ),
//             thickness,
//             lineType );

//    putText( *baseCompareMap, "x=50", tmpX, 0,1, Scalar( 100, 100, 0 ));
//    line( *baseCompareMap,
//            origin,
//            tmpX,
//            Scalar( 0, 100, 0 ),
//            2,
//            lineType );

//    Point tmpY;
//    tmpY.x = 0;
//    tmpY.y = 50;

//    circle( *baseCompareMap,
//             tmpY,
//             10,
//             Scalar( 0, 255, 0 ),
//             thickness,
//             lineType );

//    putText( *baseCompareMap, "y=50", tmpY, 0,1, Scalar( 100, 100, 0 ));
//    line( *baseCompareMap,
//            origin,
//            tmpY,
//            Scalar( 0, 100, 0 ),
//            2,
//            lineType );


//    circle( *baseCompareMap,
//             Point( local_matchX, local_matchY),
//             10,
//             Scalar( 0, 0, 255 ),
//             thickness,
//             lineType );
//    putText( *baseCompareMap, "matchPoint", Point( local_matchX, local_matchY), 0,1, Scalar( 100, 100, 0 ));

//    thickness = 2;
//     line( *baseCompareMap,
//             Point( local_matchX, local_matchY),
//             Point( local_matchX+50*cos(local_matchR), local_matchY+50*sin(local_matchR)),
//             Scalar( 0, 100, 0 ),
//             thickness,
//             lineType );
//    imshow( "baseCompareMap", *baseCompareMap );
    return 1;
}

double VisionMatch::angleRotationJudgement(double targetAngle, double initialAngle)
{
    double angle1,angle2,angle3;
    angle1=targetAngle-initialAngle;
    angle2=angle1+2*M_PI;
    angle3=angle1-2*M_PI;
    double minNum;
    if(fabs(angle1)<=fabs(angle2))
    {
        minNum=angle1;
    }
    else
    {
        minNum=angle2;
    }

    if(fabs(angle3)<=fabs(minNum))
    {
        minNum=angle3;
    }

    return minNum;
}

int VisionMatch::calculateAgvPositionProbolityError(VehiclePosition oldPoseIn, VehiclePosition newPoseIn,
                                                    VehiclePosition &poseErrorInfoOut)
{
    double delta_x=fabs(oldPoseIn.x-newPoseIn.x);
    double delta_y=fabs(oldPoseIn.y-newPoseIn.y);
    double delta_angle=fabs(angleRotationJudgement(newPoseIn.rotate,oldPoseIn.rotate));
    double length=sqrt(delta_x*delta_x+delta_y*delta_y);
    double angleError=delta_angle*odomErrorRatio_angle;
    double lErrorByAngle=length*sin(angleError);

    poseErrorInfoOut.rotate+=angleError;
    double allowAngleTolorence=30.0;
    if(fabs(poseErrorInfoOut.rotate)>allowAngleTolorence/180.0*M_PI)
    {
        poseErrorInfoOut.rotate-=angleError;
//        addMsg(ENUM_MSG_WARNING, "ReflectorMatch", "ReflectorMatch", 22205, robotId, allowAngleTolorence);
        return -1;
    }
    poseErrorInfoOut.x+=delta_x*odomErrorRatio_position+lErrorByAngle*cos(M_PI_2-oldPoseIn.rotate);
    poseErrorInfoOut.y+=delta_y*odomErrorRatio_position+lErrorByAngle*sin(M_PI_2-oldPoseIn.rotate);

    return 1;
}

int VisionMatch::calculateAgvPosition_byOdom(PositionInfo &vehiclePositionInfoOut)
{
    // agvInMapFrame=frame_odom_in_map*frame_baselink_in_odom;
  KDL::Frame agvInMapFrame=(*frame_odom_in_map)*(*frame_baselink_in_odom);
// vehiclePositionInfoOut.isUpdateSucess=false;
  vehiclePositionInfoOut.maxMatchError=fabs(poseErrorInfo.x)+fabs(poseErrorInfo.y)+fabs(poseErrorInfo.rotate);

//  vehiclePositionInfoOut.matchScore=-1;
  vehiclePositionInfoOut.position.x=agvInMapFrame.p[0];
  vehiclePositionInfoOut.position.y=agvInMapFrame.p[1];

  double tmp_roll,tmp_pitch,tmp_yaw;
  agvInMapFrame.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
  vehiclePositionInfoOut.position.rotate=tmp_yaw;

  //求激光雷达位置
//  calculateLaserLinkPosition(vehiclePositionInfoOut.position,laserlinkInMapPosition);

  //计算位置估计误差
  calculateAgvPositionProbolityError(agvInMapPosition_old.position,agvInMapPosition.position,poseErrorInfo);
  agvInMapPosition_old=agvInMapPosition;

  return 1;
}

int VisionMatch::calculateOffsetMatrix(VehiclePosition agvInMapPositionIn, KDL::Frame *frame_baselink_in_odomIn,
                                       KDL::Frame *frame_odom_in_mapOut)
{
    //agvInMapFrame
    KDL::Frame agvInMapFrame;
    agvInMapFrame.p.data[0]=agvInMapPositionIn.x;
    agvInMapFrame.p.data[1]=agvInMapPositionIn.y;
    agvInMapFrame.p.data[2]=0;
    agvInMapFrame.M=agvInMapFrame.M.RPY(0,0,agvInMapPositionIn.rotate);

    //agvInMapFrame=frame_odom_in_map*frame_baselink_in_odom;
    *frame_odom_in_mapOut=agvInMapFrame*frame_baselink_in_odomIn->Inverse();
    return 1;
}

#define D_VMARK_POSE_FILTER_COUNT 6 //lms151　medianMean效果不好
//#define D_VMARK_POSE_FILTER_COUNT 16 lms151　arithmeticMeanfilter效果不好
#define D_VMARK_POSE_FILTER_R_COUNT 8
int VisionMatch::filterPositionResult(double &pose_x,double &pose_y,double &pose_r)
{

//            pose_x=filtering_vmarkPosition_x->arithmeticMeanfilter(
//                                                        pose_x,D_VMARK_POSE_FILTER_COUNT);
//            pose_y=filtering_vmarkPosition_y->arithmeticMeanfilter(
//                                                        pose_y,D_VMARK_POSE_FILTER_COUNT);
        pose_x=filtering_vmarkPosition_x->medianMean(
                                                    pose_x,D_VMARK_POSE_FILTER_COUNT);
        pose_y=filtering_vmarkPosition_y->medianMean(
                                                    pose_y,D_VMARK_POSE_FILTER_COUNT);
        pose_r=filtering_vmarkPosition_r->medianMean_angle(
                                                    pose_r,D_VMARK_POSE_FILTER_R_COUNT);

//        pose_r=kalmanFilter->calculate_kalmanFilter(pose_r);
        return 1;

}

int VisionMatch::transBaseLinkPointToLaserPoint(double x_in, double y_in, double &x_out, double &y_out)
{
    //point 在　baselink 矩阵表示
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.Identity();
    KDL::Vector tmpVector(x_in,y_in,0);
    KDL::Frame frame_point_in_baselink(tmpRotation,tmpVector);
    qDebug()<<"frame_point_in_baselink"<<"x="<<frame_point_in_baselink.p[0]<<" ,y="<<frame_point_in_baselink.p[1]<<" ,r=";

    //baselink 在　 world 矩阵表示
    KDL::Frame frame_point_in_laser=(*frame_baselink_in_laserlink)*frame_point_in_baselink;
     qDebug()<<"frame_point_in_laser"<<"x="<<frame_point_in_laser.p[0]<<" ,y="<<frame_point_in_laser.p[1]<<" ,r=";

    x_out=frame_point_in_laser.p[0];
    y_out=frame_point_in_laser.p[1];

    return 1;
}

int VisionMatch::translateToRegion(double front, double back, double left, double right,
                                   std::vector<double> &region_x, std::vector<double> &region_y)
{
    region_x.resize(4);
    region_y.resize(4);
    region_x[0]=front;
    region_y[0]=left;
    region_x[1]=front;
    region_y[1]=-right;
    region_x[2]=-back;
    region_y[2]=-right;
    region_x[3]=-back;
    region_y[3]=left;
    //转移到车体为中心
    for(int i=0;i<region_x.size();i++)
    {
        transBaseLinkPointToLaserPoint(region_x[i],region_y[i],region_x[i],region_y[i]);
    }
    return 1;
}

int VisionMatch::calculateVmarkPositionInWorld(VehiclePosition vmarkInLaser, VehiclePosition &vmarkInWorldOut)
{
    //baselink 在　world 矩阵表示
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(0,0,baselinkReferencePosition.rotate);
//    KDL::Vector tmpVector2(localCompare_originY*mPerPix,localCompare_originX*mPerPix,0);
    KDL::Vector tmpVector(baselinkReferencePosition.x,baselinkReferencePosition.y,0);
    KDL::Frame frame_baselink_in_world(tmpRotation,tmpVector);
//    qDebug()<<"frame_laser_in_image"<<"x="<<frame_laser_in_image.p[0]<<" ,y="<<frame_laser_in_image.p[1]<<" ,r=";

    //vmark 在　laser 矩阵表示
    tmpRotation=tmpRotation.RPY(0,0,vmarkInLaser.rotate);
//    KDL::Vector tmpVector2(localCompare_originY*mPerPix,localCompare_originX*mPerPix,0);
    KDL::Vector tmpVector2(vmarkInLaser.x,vmarkInLaser.y,0);
    KDL::Frame frame_vmark_in_laser(tmpRotation,tmpVector2);
//    qDebug()<<"frame_laser_in_image"<<"x="<<frame_laser_in_image.p[0]<<" ,y="<<frame_laser_in_image.p[1]<<" ,r=";

    //vmark 在　 world 矩阵表示
    KDL::Frame frame_vmark_in_world=frame_baselink_in_world*(*frame_laserlink_in_baselink)*frame_vmark_in_laser;
//     qDebug()<<"frame_baselink_in_world"<<"x="<<frame_baselink_in_world.p[0]<<" ,y="<<frame_baselink_in_world.p[1]<<" ,r=";

    double tmp_roll,tmp_pitch,tmp_yaw;
    vmarkInWorldOut.x=frame_vmark_in_world.p[0];
    vmarkInWorldOut.y=frame_vmark_in_world.p[1];
    frame_vmark_in_world.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    vmarkInWorldOut.rotate=tmp_yaw-M_PI/2.0;//-90
    transformToPi(vmarkInWorldOut.rotate);
    return 1;
}

int VisionMatch::translate_structLine_in_laserlink_2_baselink_in_structLine(double xIn, double yIn, double rIn,int method,
                                                                            double &xOut, double &yOut, double &rOut)
{
    //structline 在　laserlink 矩阵表示
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(0,0,rIn);
//    KDL::Vector tmpVector2(localCompare_originY*mPerPix,localCompare_originX*mPerPix,0);
    KDL::Vector tmpVector(xIn,yIn,0);
    KDL::Frame frame_structline_in_laserlink(tmpRotation,tmpVector);
//    qDebug()<<"frame_laser_in_image"<<"x="<<frame_laser_in_image.p[0]<<" ,y="<<frame_laser_in_image.p[1]<<" ,r=";

    //baselink 在　 structLine 矩阵表示
    KDL::Frame frame_baselink_in_strcutline=frame_structline_in_laserlink.Inverse()*(*frame_baselink_in_laserlink);
//     qDebug()<<"frame_baselink_in_world"<<"x="<<frame_baselink_in_world.p[0]<<" ,y="<<frame_baselink_in_world.p[1]<<" ,r=";

    double tmp_roll,tmp_pitch,tmp_yaw,tmp_x,tmp_y,tmp_r;
    tmp_x=frame_baselink_in_strcutline.p[0];
    tmp_y=frame_baselink_in_strcutline.p[1];
    frame_baselink_in_strcutline.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    tmp_r=tmp_yaw;

    //修改ｓtructline坐标系,如果激光头有翻转.2个反光条的夹角中线向车的方向为ｘ
    if(fabs(laser_baslink_pitch)>3.13 && fabs(laser_baslink_pitch)<3.15)
    {
//        xOut=-xOut;
//        yOut=yOut;
//        rOut=-M_PI-rOut;
        switch(method)
        {
        case 0:
        {
            xOut=tmp_y;
            yOut=tmp_x;
            rOut=-3*M_PI/2.0-tmp_r;
            transformToPi(rOut);
            break;
        }
        case 1:
        {
            xOut=tmp_x;
            yOut=-tmp_y;
            rOut=-tmp_r;
            transformToPi(rOut);
            break;
        }
        }

    }
    else
    {
        xOut=tmp_x;
        yOut=tmp_y;
        rOut=tmp_r;
    }
    return 1;
}

int VisionMatch::translate_vmark_in_localCompareMap_2_baselink_in_world(double xIn, double yIn, double rIn,int vmarkIndexIn,
                                                                        double &xOut, double &yOut, double &rOut)
{

//    //image 在　vmark 矩阵表示
//    Eigen::AngleAxisd rollAngle(0, Eigen::Vector3d::UnitX());
//    Eigen::AngleAxisd pitchAngle(0, Eigen::Vector3d::UnitY());
//    Eigen::AngleAxisd yawAngle(rIn, Eigen::Vector3d::UnitZ());

//    Eigen::Quaterniond q = yawAngle*pitchAngle*rollAngle;
//    Eigen::Matrix3d rotateMa = q.matrix();

//    Eigen::Isometry3d eigen_frame_vmark_in_image;
//    eigen_frame_vmark_in_image= rotateMa;
//    eigen_frame_vmark_in_image.translation()=Eigen::Vector3d(xIn*mPerPix,yIn*mPerPix,0);
//    Eigen::Isometry3d eigen_frame_image_in_vmark=eigen_frame_vmark_in_image.inverse();

//    //vmark 在　 world 矩阵表示
//    Eigen::AngleAxisd rollAngle2(0, Eigen::Vector3d::UnitX());
//    Eigen::AngleAxisd pitchAngle2(0, Eigen::Vector3d::UnitY());
//    Eigen::AngleAxisd yawAngle2(vmarkPosition.value(vmarkIndexIn).pose_r/180.0*M_PI, Eigen::Vector3d::UnitZ());

//    Eigen::Quaterniond q2 = yawAngle2*pitchAngle2*rollAngle2;
//    Eigen::Matrix3d rotateMa2 = q2.matrix();

//    Eigen::Isometry3d eigen_frame_vmark_in_world;
//    eigen_frame_vmark_in_world= rotateMa2;
//    eigen_frame_vmark_in_world.translation()=Eigen::Vector3d(vmarkPosition.value(vmarkIndexIn).pose_x/1000.0,
//                                                             vmarkPosition.value(vmarkIndexIn).pose_y/1000.0, 0);


//    //image 在　 world 矩阵表示
//    Eigen::Isometry3d  eigen_frame_image_in_world=eigen_frame_vmark_in_world*eigen_frame_image_in_vmark;

//    qDebug()<<"eigen frame_image_in_world"<<"x="<<eigen_frame_image_in_world.translation()[0]<<" ,y="<<eigen_frame_image_in_world.translation()[1]<<" ,r=";



    //-------------------------------------------------------
    //todo 角度是对的，世界ｘ方向是对的，ｙ方向差０．５，车体旋转后也是这样。
    //image 在　vmark 矩阵表示
    KDL::Rotation tmpRotation;
//    tmpRotation=tmpRotation.RPY(0,0,-(rIn-M_PI/2));
//    tmpRotation=tmpRotation.RPY(0,0,-(rIn+M_PI/2));
//    tmpRotation=tmpRotation.RPY(0,0,-(rIn+3*M_PI/2));
    tmpRotation=tmpRotation.RPY(0,0,rIn);//?
    KDL::Vector tmpVector(xIn*mPerPix,yIn*mPerPix,0);
//    KDL::Vector tmpVector(yIn*mPerPix,xIn*mPerPix,0);//?为什么
    KDL::Frame frame_vmark_in_image(tmpRotation,tmpVector);
    KDL::Frame frame_image_in_vmark=frame_vmark_in_image.Inverse();
//    qDebug()<<"111frame_image_in_vmark"<<"x="<<frame_image_in_vmark.p[0]<<" ,y="<<frame_image_in_vmark.p[1]<<" ,r=";
//    qDebug()<<"rIn="<<rIn;
    double tmp_roll5,tmp_pitch5,tmp_yaw5;
    frame_image_in_vmark.M.GetRPY(tmp_roll5,tmp_pitch5,tmp_yaw5);
//    qDebug()<<"tmp_roll5"<<tmp_roll5<<"tmp_pitch5"<<tmp_pitch5<<"tmp_yaw5"<<tmp_yaw5;


    //laser 在　image 矩阵表示
    tmpRotation=tmpRotation.RPY(0,0,0);
//    KDL::Vector tmpVector2(localCompare_originY*mPerPix,localCompare_originX*mPerPix,0);
    KDL::Vector tmpVector2(localCompare_originX*mPerPix,localCompare_originY*mPerPix,0);
    KDL::Frame frame_laser_in_image(tmpRotation,tmpVector2);
//    qDebug()<<"frame_laser_in_image"<<"x="<<frame_laser_in_image.p[0]<<" ,y="<<frame_laser_in_image.p[1]<<" ,r=";

    //vmark 在　 world 矩阵表示
    if(false==vmarkPosition.contains(vmarkIndexIn))
    {
        qDebug()<<"error,VisionMatch translate_vmark_in_localCompareMap_2_baselink_in_world";
        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8233, robotId, vmarkIndexIn);
        return -1;
    }
    if(currentVmarkType!=vmarkPosition.value(vmarkIndexIn).vmarkType)
    {
        qDebug()<<"error,VisionMatch translate_vmark_in_localCompareMap_2_baselink_in_world";
        addMsg(ENUM_MSG_ERROR, "VisionMatch", "VisionMatch", 8235, robotId, currentVmarkType,vmarkPosition.value(vmarkIndexIn).vmarkType);
        return -2;
    }
    tmpRotation=tmpRotation.RPY(0,0,vmarkPosition.value(vmarkIndexIn).pose_r/180.0*M_PI);
    KDL::Vector tmpVector3(vmarkPosition.value(vmarkIndexIn).pose_x/1000.0,vmarkPosition.value(vmarkIndexIn).pose_y/1000.0, 0);
    KDL::Frame frame_vmark_in_world(tmpRotation,tmpVector3);
//     qDebug()<<"frame_vmark_in_world"<<"x="<<frame_vmark_in_world.p[0]<<" ,y="<<frame_vmark_in_world.p[1]<<" ,r=";

     //image 在　 world 矩阵表示
//     KDL::Frame frame_image_in_world=frame_vmark_in_world*frame_image_in_vmark;
//     qDebug()<<"frame_image_in_world"<<"x="<<frame_image_in_world.p[0]<<" ,y="<<frame_image_in_world.p[1]<<" ,r=";
     //laser 在　 world 矩阵表示
//     KDL::Frame frame_laser_in_world=frame_vmark_in_world*frame_image_in_vmark*frame_laser_in_image;
//     qDebug()<<"frame_laser_in_world"<<"x="<<frame_laser_in_world.p[0]<<" ,y="<<frame_laser_in_world.p[1]<<" ,r=";

    //baselink 在　 world 矩阵表示
    KDL::Frame frame_baselink_in_world=frame_vmark_in_world*frame_image_in_vmark*frame_laser_in_image*(*frame_baselink_in_laserlink);
//     qDebug()<<"frame_baselink_in_world"<<"x="<<frame_baselink_in_world.p[0]<<" ,y="<<frame_baselink_in_world.p[1]<<" ,r=";

    double tmp_roll,tmp_pitch,tmp_yaw;
    xOut=frame_baselink_in_world.p[0];
    yOut=frame_baselink_in_world.p[1];
    frame_baselink_in_world.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
//    rOut=tmp_yaw;//todo
    rOut=-tmp_yaw;//todo

    return 1;
}

int VisionMatch::translate_vmark_in_localCompareMap_2_vmark_in_world(double xIn, double yIn, double rIn,
                                               double x_baselink, double y_baselink, double r_baselink,
                                                                     double &xOut, double &yOut, double &rOut)
{
    //baselink 在　world 矩阵表示
    KDL::Rotation tmpRotation;
    tmpRotation=tmpRotation.RPY(0,0,r_baselink);
    KDL::Vector tmpVector(x_baselink,y_baselink,0);
    KDL::Frame frame_baselink_in_world(tmpRotation,tmpVector);
//    qDebug()<<"111frame_baselink_in_world"<<"x="<<x_baselink<<" ,y="<<y_baselink<<" ,r="<<r_baselink;

    //image 在　 laser 矩阵表示
    tmpRotation=tmpRotation.RPY(0,0,0);
    KDL::Vector tmpVector2(-localCompare_originX*mPerPix,-localCompare_originY*mPerPix,0);
    KDL::Frame frame_image_in_laser(tmpRotation,tmpVector2);
//    qDebug()<<"frame_image_in_laser"<<"x="<<-localCompare_originX*mPerPix<<" ,y="<<-localCompare_originY*mPerPix<<" ,r=0";

    //vmark 在　 image矩阵表示
    tmpRotation=tmpRotation.RPY(0,0,rIn);
    KDL::Vector tmpVector3(xIn*mPerPix,yIn*mPerPix,0);
    KDL::Frame frame_vmark_in_image(tmpRotation,tmpVector3);
//    qDebug()<<"frame_vmark_in_image"<<"x="<<xIn*mPerPix<<" ,y="<<yIn*mPerPix<<" ,r="<<rIn;

//        //vmark 在　 laser 矩阵表示
//        tmpRotation=tmpRotation.RPY(0,0,rIn);
//        KDL::Vector tmpVector3(xIn*mPerPix,yIn*mPerPix,0);
//        KDL::Frame frame_vmark_in_image(tmpRotation,tmpVector3);

//    qDebug()<<"frame_laserlink_in_baselink"<<"x="<<frame_laserlink_in_baselink->p[0]<<" ,y="<<frame_laserlink_in_baselink->p[1]<<" ,r=";

    //laser 在　 world 矩阵表示
//    KDL::Frame frame_laser_in_world=frame_baselink_in_world*(*frame_laserlink_in_baselink);
//    qDebug()<<"frame_laser_in_world"<<"x="<<frame_laser_in_world.p[0]<<" ,y="<<frame_laser_in_world.p[1]<<" ,r=";

//    //image 在　 world 矩阵表示
//    KDL::Frame frame_image_in_world=frame_baselink_in_world*(*frame_laserlink_in_baselink)*frame_image_in_laser;
//    qDebug()<<"frame_image_in_world"<<"x="<<frame_image_in_world.p[0]<<" ,y="<<frame_image_in_world.p[1]<<" ,r=";

    //vmark 在　 world 矩阵表示
    KDL::Frame frame_vmark_in_world=frame_baselink_in_world*(*frame_laserlink_in_baselink)*frame_image_in_laser*frame_vmark_in_image;
//    KDL::Frame frame_vmark_in_world=frame_vmark_in_image*frame_image_in_laser*(*frame_laserlink_in_baselink)*frame_baselink_in_world;


    double tmp_roll,tmp_pitch,tmp_yaw;
    xOut=frame_vmark_in_world.p[0];
    yOut=frame_vmark_in_world.p[1];
    frame_vmark_in_world.M.GetRPY(tmp_roll,tmp_pitch,tmp_yaw);
    rOut=tmp_yaw;


    return 1;
}



int VisionMatch::readVMarkConfigFile(QString filePathIn,QMap<int,VMarkDetectParameter>&vmark_detect_Parameter_out)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, VisionMatch::readConfigFile open file error！";
       return -1;
    }

    QDomNode  inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "VMark", ok);
    if( !ok )
    {
        qDebug()<<"error, VMark node error！";
        return -1;
    }
    QDomNodeList tmpNodelist=inNode.childNodes();
    int tmp_vmarkType;
    VMarkDetectParameter tmp_vmarParmeter;
    vmark_detect_Parameter_out.clear();
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 tmp_vmarkType
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "vmarkType", ok), tmp_vmarkType );
        if( !ok )
        {
            qDebug()<<"error, tmp_vmarkType node error！";
            return -1;
        }

        //-------------read LineDetectParameter----------
        inNode = xmlParser.findSubNode(tmpNodelist.at(i), "LineDetectParameter", ok );
        if( !ok )
        {

            qDebug()<<"error, VisionMatch::readConfigFile node error！";
            return -1;
        }

        //节点ｒｅａｄ数据
        xmlParser.readXml( xmlParser.findSubNode(inNode, "rho_distanceAccuracy", ok), tmp_vmarParmeter.rho_distanceAccuracy  );
        if( !ok )
        {
            qDebug()<<"error, rho_distanceAccuracy node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "theta_angleAccuracy", ok), tmp_vmarParmeter.theta_angleAccuracy  );
        if( !ok )
        {
            qDebug()<<"error, theta_angleAccuracy node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "threshold_pt", ok), tmp_vmarParmeter.threshold_pt  );
        if( !ok )
        {
            qDebug()<<"error, threshold_pt node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "minLineLength", ok), tmp_vmarParmeter.minLineLength  );
        if( !ok )
        {
            qDebug()<<"error, minLineLength node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "maxLineGap", ok), tmp_vmarParmeter.maxLineGap  );
        if( !ok )
        {
            qDebug()<<"error, maxLineGap node error！";
            return -1;
        }

        //-------------read VMarkDetectParameter----------
        inNode = xmlParser.findSubNode(tmpNodelist.at(i), "VMarkDetectParameter", ok );
        if( !ok )
        {

            qDebug()<<"error, VisionMatch::readConfigFile node error！";
            return -1;
        }

        //节点ｒｅａｄ数据
        xmlParser.readXml( xmlParser.findSubNode(inNode, "vmark_min_angle", ok), tmp_vmarParmeter.vmark_min_angle  );
        if( !ok )
        {
            qDebug()<<"error, vmark_min_angle node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "vmark_max_angle", ok), tmp_vmarParmeter.vmark_max_angle  );
        if( !ok )
        {
            qDebug()<<"error, vmark_max_angle node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "vmark_max_cross_distance", ok), tmp_vmarParmeter.vmark_max_cross_distance  );
        if( !ok )
        {
            qDebug()<<"error, vmark_max_cross_distance node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "vmark_allow_vDistance", ok), tmp_vmarParmeter.vmark_allow_vDistance  );
        if( !ok )
        {
            qDebug()<<"error, vmark_allow_vDistance node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(inNode, "vmark_allow_vAngle", ok), tmp_vmarParmeter.vmark_allow_vAngle  );
        if( !ok )
        {
            qDebug()<<"error, vmark_allow_vAngle node error！";
            return -1;
        }
        vmark_detect_Parameter_out.insert(tmp_vmarkType,tmp_vmarParmeter);
    }


    xmlParser.closeXml( );

    return 1;
}

int VisionMatch::readStructLineConfigFile(QString filePathIn, QMap<int, StructLineParameter> &structLine_Parameter_out,
                                          QMap<int,LaserFilterParameter> &laserFilter_ParameterOut)
{
    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, VisionMatch::readConfigFile open file error！";
       return -1;
    }

    QDomNode  inNode;
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "StructLine", ok);
    if( !ok )
    {
        qDebug()<<"error, StructLine node error！";
        return -1;
    }
    QDomNodeList tmpNodelist=inNode.childNodes();
    int structLineType;
    StructLineParameter tmp_lineParmeter;
    structLine_Parameter_out.clear();
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 structLineType
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "structLineType", ok), structLineType );
        if( !ok )
        {
            qDebug()<<"error, structLineType node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minGap", ok), tmp_lineParmeter.minGap );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxGap", ok), tmp_lineParmeter.maxGap );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minAngle", ok), tmp_lineParmeter.minAngle );
        tmp_lineParmeter.minAngle =tmp_lineParmeter.minAngle*M_PI/180.0;
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxAngle", ok), tmp_lineParmeter.maxAngle );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
        tmp_lineParmeter.maxAngle =tmp_lineParmeter.maxAngle*M_PI/180.0;
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "firstLineMinLength", ok), tmp_lineParmeter.firstLineMinLength );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "firstLineMaxLength", ok), tmp_lineParmeter.firstLineMaxLength );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
//        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "secondLineMinLength", ok), tmp_lineParmeter.secondLineMinLength );
//        if( !ok )
//        {
//            qDebug()<<"error,  node error！";
//            return -1;
//        }
//        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "secondLineMaxLength", ok), tmp_lineParmeter.secondLineMaxLength );
//        if( !ok )
//        {
//            qDebug()<<"error,  node error！";
//            return -1;
//        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "allowMaxDeltaDistance", ok), tmp_lineParmeter.allowMaxDeltaDistance );
        if( !ok )
        {
            qDebug()<<"error,  node error！";
            return -1;
        }
//        int tmp_isFit;
//        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "isFittingLine", ok), tmp_isFit );
//        if( !ok )
//        {
//            qDebug()<<"error,  node error！";
//            return -1;
//        }
//        tmp_lineParmeter.isFittingLine=(bool)tmp_isFit;
//        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "segmentJudgeMethod", ok), tmp_lineParmeter.segmentJudgeMethod );
//        if( !ok )
//        {
//            qDebug()<<"error,  node error！";
//            return -1;
//        }
//        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "allowIntensityBias", ok), tmp_lineParmeter.allowIntensityBias );
//        if( !ok )
//        {
//            qDebug()<<"error,  node error！";
//            return -1;
//        }




        structLine_Parameter_out.insert(structLineType,tmp_lineParmeter);
    }

    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "LaserFilter", ok);
    if( !ok )
    {
        qDebug()<<"error, LaserFilter node error！";
        return -1;
    }
    tmpNodelist=inNode.childNodes();
    int filterType;
    LaserFilterParameter tmp_filterParmeter;
    laserFilter_ParameterOut.clear();
    for(int i=0;i<tmpNodelist.size();i++)
    {
        //节点ｒｅａｄ数据 filterType
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "filterType", ok), filterType );
        if( !ok )
        {
            qDebug()<<"error, structLineType node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minumIntensity", ok), tmp_filterParmeter.minumIntensity );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxLaserRange", ok), tmp_filterParmeter.maxLaserRange );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minLaserRange", ok), tmp_filterParmeter.minLaserRange );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "minAngle", ok), tmp_filterParmeter.minAngle );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "maxAngle", ok), tmp_filterParmeter.maxAngle );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_front", ok), tmp_filterParmeter.region_front );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_back", ok), tmp_filterParmeter.region_back );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_left", ok), tmp_filterParmeter.region_left );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(tmpNodelist.at(i), "region_right", ok), tmp_filterParmeter.region_right );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        laserFilter_ParameterOut.insert(filterType,tmp_filterParmeter);
    }


    //CalculateNode
    inNode=xmlParser.findSubNode(xmlParser.getRootDomElement(), "CalculateNode", ok);
    if( !ok )
    {
        qDebug()<<"error, CalculateNode node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "odomErrorRatio_position", ok), odomErrorRatio_position );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.readXml( xmlParser.findSubNode(inNode, "odomErrorRatio_angle", ok), odomErrorRatio_angle );
    if( !ok )
    {
        qDebug()<<"error,  node error！";
        return -1;
    }

    xmlParser.closeXml( );

    return 1;
}

int VisionMatch::readVMarkFile(QString filePathIn, QMap<int, VMarkInfo> &vmarkPositionOut)
{
    vmarkPositionOut.clear();

    DomParser xmlParser;

    bool ok = xmlParser.openXml(filePathIn, QIODevice::ReadOnly );
    if( !ok )//无法打开文件！
    {
       qDebug()<<"error, VisionMatch::readVMarkFile open file error！";
       return -1;
    }

    QDomNodeList  nodeList;

    nodeList = xmlParser.getRootDomElement().childNodes();

    VMarkInfo tmp_markInfo;

    for(int i=0;i<nodeList.size();i++)
    {
        //节点ｒｅａｄ数据
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "comment", ok), tmp_markInfo.comment  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "vindex", ok), tmp_markInfo.vindex  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "vmarkType", ok), tmp_markInfo.vmarkType  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "mapName", ok), tmp_markInfo.mapName  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "pose_x", ok), tmp_markInfo.pose_x  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "pose_y", ok), tmp_markInfo.pose_y  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }
        xmlParser.readXml( xmlParser.findSubNode(nodeList.at(i), "pose_r", ok), tmp_markInfo.pose_r  );
        if( !ok )
        {
            qDebug()<<"error, node error！";
            return -1;
        }

        vmarkPositionOut.insert(tmp_markInfo.vindex,tmp_markInfo);

    }


    xmlParser.closeXml( );

    return 1;
}

int VisionMatch::translateBaseMapImagePointToWorldPoint(vector<double> x_in, vector<double> y_in, vector<double> r_in,
                                               vector<double> &x_out, vector<double> &y_out, vector<double> &r_out)
{
    int tmpCount=x_in.size();
    x_out.resize(tmpCount);
    y_out.resize(tmpCount);
    r_out.resize(tmpCount);
    for(int i=0;i<tmpCount;i++)
    {
        translateBaseMapImagePointToWorldPoint(x_in[i],y_in[i],r_in[i],x_out[i],y_out[i],r_out[i]);
    }
    return 1;
}

int VisionMatch::translateBaseMapImagePointToWorldPoint(double x_in, double y_in, double r_in, double &x_out, double &y_out, double &r_out)
{
    x_out=(x_in-baseMap_originX)*mPerPix;
    y_out=(y_in-baseMap_originY)*mPerPix;
    r_out=r_in;
    return 1;
}

int VisionMatch::translateWorldPointToBaseMapImagePoint(double x_in, double y_in, double r_in, double &x_out, double &y_out, double &r_out)
{
    x_out=x_in/mPerPix+baseMap_originX;
    y_out=y_in/mPerPix+baseMap_originY;
    r_out=r_in;
    return 1;
}

void VisionMatch::addMsg(int messageLevel, std::string componentName, std::string messageType
                              , int messageCode, int robotIdIn
                              , double parameter1, double parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;
    QString tmpStr;

    switch(messageCode)
    {
    case 8237:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",反光条检查出错，激光过滤类型(%1)没有存储数据　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8236:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",反光条检查出错，反光条类型(%1)没有存储数据　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8235:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",vmark检查出错，用户声明的ｖmark类型(%1)与系统存储的ｖmark类型(%1)矛盾　！").arg(parameter1).arg(parameter2);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8234:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",vmark检查出错，vmark类型(%1)没有存储数据　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8233:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",vmark检查出错，vmark号码(%1)没有数据记录　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8232:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",vmark检查到VMARK数量(%1)大于１　！").arg(parameter1);
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8231:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",vmark检查不到任何VMARK　！");
        infomationStr = tmpStr.toStdString();
        break;
    }
    case 8230:
    {
        tmpStr =  QObject::tr("机器人:")  + QString::number(robotId);
        tmpStr = tmpStr + QObject::tr(",visionMatch.xml文件读取失败　！");
        infomationStr = tmpStr.toStdString();
        break;
    }



        default:
        {
            qDebug()<<"Coordinate::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    MessageLog::getInstance()->addMessage(tmpMsg);
    #endif
}

